1 package com.irurueta.navigation.inertial.calibration.accelerometer;
2
3 import com.irurueta.algebra.Matrix;
4 import com.irurueta.algebra.WrongSizeException;
5 import com.irurueta.navigation.LockedException;
6 import com.irurueta.navigation.NotReadyException;
7 import com.irurueta.navigation.frames.CoordinateTransformation;
8 import com.irurueta.navigation.frames.ECEFFrame;
9 import com.irurueta.navigation.frames.FrameType;
10 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
11 import com.irurueta.navigation.frames.NEDFrame;
12 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
13 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
14 import com.irurueta.navigation.geodesic.Constants;
15 import com.irurueta.navigation.inertial.BodyKinematics;
16 import com.irurueta.navigation.inertial.ECEFGravity;
17 import com.irurueta.navigation.inertial.ECEFPosition;
18 import com.irurueta.navigation.inertial.ECEFVelocity;
19 import com.irurueta.navigation.inertial.NEDPosition;
20 import com.irurueta.navigation.inertial.NEDVelocity;
21 import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
22 import com.irurueta.navigation.inertial.calibration.CalibrationException;
23 import com.irurueta.navigation.inertial.calibration.IMUErrors;
24 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
25 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
26 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
27 import com.irurueta.statistics.UniformRandomizer;
28 import com.irurueta.units.Acceleration;
29 import com.irurueta.units.AccelerationUnit;
30 import org.junit.Test;
31
32 import java.util.ArrayList;
33 import java.util.Collection;
34 import java.util.Collections;
35 import java.util.List;
36 import java.util.Random;
37
38 import static org.junit.Assert.*;
39
40 public class KnownPositionAccelerometerCalibratorTest implements
41 KnownPositionAccelerometerCalibratorListener {
42
43 private static final double TIME_INTERVAL_SECONDS = 0.02;
44
45 private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
46 private static final double DEG_TO_RAD = 0.01745329252;
47
48 private static final double MIN_ANGLE_DEGREES = -180.0;
49 private static final double MAX_ANGLE_DEGREES = 180.0;
50
51 private static final double MIN_LATITUDE_DEGREES = -90.0;
52 private static final double MAX_LATITUDE_DEGREEs = 90.0;
53 private static final double MIN_LONGITUDE_DEGREES = -180.0;
54 private static final double MAX_LONGITUDE_DEGREES = 180.0;
55 private static final double MIN_HEIGHT = -50.0;
56 private static final double MAX_HEIGHT = 50.0;
57
58 private static final int LARGE_MEASUREMENT_NUMBER = 100000;
59
60 private static final double ABSOLUTE_ERROR = 1e-8;
61 private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
62 private static final double VERY_LARGE_ABSOLUTE_ERROR = 1e-3;
63
64 private static final int TIMES = 100;
65
66 private int mCalibrateStart;
67 private int mCalibrateEnd;
68
69 @Test
70 public void testConstructor1() throws WrongSizeException {
71 final KnownPositionAccelerometerCalibrator calibrator =
72 new KnownPositionAccelerometerCalibrator();
73
74
75 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
76 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
77 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
78 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
79 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
80 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
81 final Acceleration bx2 = new Acceleration(0.0,
82 AccelerationUnit.FEET_PER_SQUARED_SECOND);
83 calibrator.getInitialBiasXAsAcceleration(bx2);
84 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
85 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
86 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
87 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
88 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
89 final Acceleration by2 = new Acceleration(0.0,
90 AccelerationUnit.FEET_PER_SQUARED_SECOND);
91 calibrator.getInitialBiasYAsAcceleration(by2);
92 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
93 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
94 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
95 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
96 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
97 final Acceleration bz2 = new Acceleration(0.0,
98 AccelerationUnit.FEET_PER_SQUARED_SECOND);
99 calibrator.getInitialBiasZAsAcceleration(bz2);
100 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
101 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
102 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
103 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
104 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
105 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
106 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
107 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
108 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
109 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
110 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
111 final double[] bias1 = calibrator.getInitialBias();
112 assertArrayEquals(bias1, new double[3], 0.0);
113 final double[] bias2 = new double[3];
114 calibrator.getInitialBias(bias2);
115 assertArrayEquals(bias1, bias2, 0.0);
116 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
117 assertEquals(b1, new Matrix(3, 1));
118 final Matrix b2 = new Matrix(3, 1);
119 calibrator.getInitialBiasAsMatrix(b2);
120 assertEquals(b1, b2);
121 final Matrix ma1 = calibrator.getInitialMa();
122 assertEquals(ma1, new Matrix(3, 3));
123 final Matrix ma2 = new Matrix(3, 3);
124 calibrator.getInitialMa(ma2);
125 assertEquals(ma1, ma2);
126 assertNull(calibrator.getEcefPosition());
127 assertNull(calibrator.getNedPosition());
128 assertFalse(calibrator.getNedPosition(null));
129 assertNull(calibrator.getMeasurements());
130 assertFalse(calibrator.isCommonAxisUsed());
131 assertNull(calibrator.getListener());
132 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
133 assertFalse(calibrator.isReady());
134 assertFalse(calibrator.isRunning());
135 assertNull(calibrator.getEstimatedBiases());
136 assertFalse(calibrator.getEstimatedBiases(null));
137 assertNull(calibrator.getEstimatedBiasesAsMatrix());
138 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
139 assertNull(calibrator.getEstimatedBiasFx());
140 assertNull(calibrator.getEstimatedBiasFy());
141 assertNull(calibrator.getEstimatedBiasFz());
142 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
143 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
144 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
145 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
146 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
147 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
148 assertNull(calibrator.getEstimatedMa());
149 assertNull(calibrator.getEstimatedSx());
150 assertNull(calibrator.getEstimatedSy());
151 assertNull(calibrator.getEstimatedSz());
152 assertNull(calibrator.getEstimatedMxy());
153 assertNull(calibrator.getEstimatedMxz());
154 assertNull(calibrator.getEstimatedMyx());
155 assertNull(calibrator.getEstimatedMyz());
156 assertNull(calibrator.getEstimatedMzx());
157 assertNull(calibrator.getEstimatedMzy());
158 assertNull(calibrator.getEstimatedCovariance());
159 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
160 assertNull(calibrator.getGroundTruthGravityNorm());
161 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
162 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
163 }
164
165 @Test
166 public void testConstructor2() throws WrongSizeException {
167 final KnownPositionAccelerometerCalibrator calibrator =
168 new KnownPositionAccelerometerCalibrator(this);
169
170
171 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
172 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
173 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
174 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
175 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
176 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
177 final Acceleration bx2 = new Acceleration(0.0,
178 AccelerationUnit.FEET_PER_SQUARED_SECOND);
179 calibrator.getInitialBiasXAsAcceleration(bx2);
180 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
181 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
182 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
183 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
184 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
185 final Acceleration by2 = new Acceleration(0.0,
186 AccelerationUnit.FEET_PER_SQUARED_SECOND);
187 calibrator.getInitialBiasYAsAcceleration(by2);
188 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
189 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
190 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
191 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
192 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
193 final Acceleration bz2 = new Acceleration(0.0,
194 AccelerationUnit.FEET_PER_SQUARED_SECOND);
195 calibrator.getInitialBiasZAsAcceleration(bz2);
196 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
197 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
198 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
199 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
200 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
201 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
202 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
203 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
204 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
205 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
206 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
207 final double[] bias1 = calibrator.getInitialBias();
208 assertArrayEquals(bias1, new double[3], 0.0);
209 final double[] bias2 = new double[3];
210 calibrator.getInitialBias(bias2);
211 assertArrayEquals(bias1, bias2, 0.0);
212 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
213 assertEquals(b1, new Matrix(3, 1));
214 final Matrix b2 = new Matrix(3, 1);
215 calibrator.getInitialBiasAsMatrix(b2);
216 assertEquals(b1, b2);
217 final Matrix ma1 = calibrator.getInitialMa();
218 assertEquals(ma1, new Matrix(3, 3));
219 final Matrix ma2 = new Matrix(3, 3);
220 calibrator.getInitialMa(ma2);
221 assertEquals(ma1, ma2);
222 assertNull(calibrator.getEcefPosition());
223 assertNull(calibrator.getNedPosition());
224 assertFalse(calibrator.getNedPosition(null));
225 assertNull(calibrator.getMeasurements());
226 assertFalse(calibrator.isCommonAxisUsed());
227 assertSame(calibrator.getListener(), this);
228 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
229 assertFalse(calibrator.isReady());
230 assertFalse(calibrator.isRunning());
231 assertNull(calibrator.getEstimatedBiases());
232 assertFalse(calibrator.getEstimatedBiases(null));
233 assertNull(calibrator.getEstimatedBiasesAsMatrix());
234 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
235 assertNull(calibrator.getEstimatedBiasFx());
236 assertNull(calibrator.getEstimatedBiasFy());
237 assertNull(calibrator.getEstimatedBiasFz());
238 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
239 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
240 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
241 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
242 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
243 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
244 assertNull(calibrator.getEstimatedMa());
245 assertNull(calibrator.getEstimatedSx());
246 assertNull(calibrator.getEstimatedSy());
247 assertNull(calibrator.getEstimatedSz());
248 assertNull(calibrator.getEstimatedMxy());
249 assertNull(calibrator.getEstimatedMxz());
250 assertNull(calibrator.getEstimatedMyx());
251 assertNull(calibrator.getEstimatedMyz());
252 assertNull(calibrator.getEstimatedMzx());
253 assertNull(calibrator.getEstimatedMzy());
254 assertNull(calibrator.getEstimatedCovariance());
255 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
256 assertNull(calibrator.getGroundTruthGravityNorm());
257 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
258 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
259 }
260
261 @Test
262 public void testConstructor3() throws WrongSizeException {
263 final Collection<StandardDeviationBodyKinematics> measurements =
264 Collections.emptyList();
265 final KnownPositionAccelerometerCalibrator calibrator =
266 new KnownPositionAccelerometerCalibrator(measurements);
267
268
269 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
270 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
271 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
272 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
273 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
274 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
275 final Acceleration bx2 = new Acceleration(0.0,
276 AccelerationUnit.FEET_PER_SQUARED_SECOND);
277 calibrator.getInitialBiasXAsAcceleration(bx2);
278 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
279 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
280 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
281 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
282 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
283 final Acceleration by2 = new Acceleration(0.0,
284 AccelerationUnit.FEET_PER_SQUARED_SECOND);
285 calibrator.getInitialBiasYAsAcceleration(by2);
286 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
287 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
288 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
289 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
290 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
291 final Acceleration bz2 = new Acceleration(0.0,
292 AccelerationUnit.FEET_PER_SQUARED_SECOND);
293 calibrator.getInitialBiasZAsAcceleration(bz2);
294 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
295 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
296 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
297 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
298 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
299 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
300 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
301 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
302 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
303 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
304 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
305 final double[] bias1 = calibrator.getInitialBias();
306 assertArrayEquals(bias1, new double[3], 0.0);
307 final double[] bias2 = new double[3];
308 calibrator.getInitialBias(bias2);
309 assertArrayEquals(bias1, bias2, 0.0);
310 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
311 assertEquals(b1, new Matrix(3, 1));
312 final Matrix b2 = new Matrix(3, 1);
313 calibrator.getInitialBiasAsMatrix(b2);
314 assertEquals(b1, b2);
315 final Matrix ma1 = calibrator.getInitialMa();
316 assertEquals(ma1, new Matrix(3, 3));
317 final Matrix ma2 = new Matrix(3, 3);
318 calibrator.getInitialMa(ma2);
319 assertEquals(ma1, ma2);
320 assertNull(calibrator.getEcefPosition());
321 assertNull(calibrator.getNedPosition());
322 assertFalse(calibrator.getNedPosition(null));
323 assertSame(calibrator.getMeasurements(), measurements);
324 assertFalse(calibrator.isCommonAxisUsed());
325 assertNull(calibrator.getListener());
326 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
327 assertFalse(calibrator.isReady());
328 assertFalse(calibrator.isRunning());
329 assertNull(calibrator.getEstimatedBiases());
330 assertFalse(calibrator.getEstimatedBiases(null));
331 assertNull(calibrator.getEstimatedBiasesAsMatrix());
332 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
333 assertNull(calibrator.getEstimatedBiasFx());
334 assertNull(calibrator.getEstimatedBiasFy());
335 assertNull(calibrator.getEstimatedBiasFz());
336 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
337 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
338 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
339 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
340 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
341 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
342 assertNull(calibrator.getEstimatedMa());
343 assertNull(calibrator.getEstimatedSx());
344 assertNull(calibrator.getEstimatedSy());
345 assertNull(calibrator.getEstimatedSz());
346 assertNull(calibrator.getEstimatedMxy());
347 assertNull(calibrator.getEstimatedMxz());
348 assertNull(calibrator.getEstimatedMyx());
349 assertNull(calibrator.getEstimatedMyz());
350 assertNull(calibrator.getEstimatedMzx());
351 assertNull(calibrator.getEstimatedMzy());
352 assertNull(calibrator.getEstimatedCovariance());
353 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
354 assertNull(calibrator.getGroundTruthGravityNorm());
355 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
356 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
357 }
358
359 @Test
360 public void testConstructor4() throws WrongSizeException {
361 final Collection<StandardDeviationBodyKinematics> measurements =
362 Collections.emptyList();
363 final KnownPositionAccelerometerCalibrator calibrator =
364 new KnownPositionAccelerometerCalibrator(measurements, this);
365
366
367 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
368 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
369 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
370 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
371 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
372 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
373 final Acceleration bx2 = new Acceleration(0.0,
374 AccelerationUnit.FEET_PER_SQUARED_SECOND);
375 calibrator.getInitialBiasXAsAcceleration(bx2);
376 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
377 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
378 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
379 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
380 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
381 final Acceleration by2 = new Acceleration(0.0,
382 AccelerationUnit.FEET_PER_SQUARED_SECOND);
383 calibrator.getInitialBiasYAsAcceleration(by2);
384 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
385 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
386 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
387 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
388 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
389 final Acceleration bz2 = new Acceleration(0.0,
390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
391 calibrator.getInitialBiasZAsAcceleration(bz2);
392 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
393 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
394 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
395 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
396 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
397 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
398 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
399 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
400 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
401 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
402 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
403 final double[] bias1 = calibrator.getInitialBias();
404 assertArrayEquals(bias1, new double[3], 0.0);
405 final double[] bias2 = new double[3];
406 calibrator.getInitialBias(bias2);
407 assertArrayEquals(bias1, bias2, 0.0);
408 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
409 assertEquals(b1, new Matrix(3, 1));
410 final Matrix b2 = new Matrix(3, 1);
411 calibrator.getInitialBiasAsMatrix(b2);
412 assertEquals(b1, b2);
413 final Matrix ma1 = calibrator.getInitialMa();
414 assertEquals(ma1, new Matrix(3, 3));
415 final Matrix ma2 = new Matrix(3, 3);
416 calibrator.getInitialMa(ma2);
417 assertEquals(ma1, ma2);
418 assertNull(calibrator.getEcefPosition());
419 assertNull(calibrator.getNedPosition());
420 assertFalse(calibrator.getNedPosition(null));
421 assertSame(calibrator.getMeasurements(), measurements);
422 assertFalse(calibrator.isCommonAxisUsed());
423 assertSame(calibrator.getListener(), this);
424 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
425 assertFalse(calibrator.isReady());
426 assertFalse(calibrator.isRunning());
427 assertNull(calibrator.getEstimatedBiases());
428 assertFalse(calibrator.getEstimatedBiases(null));
429 assertNull(calibrator.getEstimatedBiasesAsMatrix());
430 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
431 assertNull(calibrator.getEstimatedBiasFx());
432 assertNull(calibrator.getEstimatedBiasFy());
433 assertNull(calibrator.getEstimatedBiasFz());
434 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
435 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
436 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
437 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
438 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
439 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
440 assertNull(calibrator.getEstimatedMa());
441 assertNull(calibrator.getEstimatedSx());
442 assertNull(calibrator.getEstimatedSy());
443 assertNull(calibrator.getEstimatedSz());
444 assertNull(calibrator.getEstimatedMxy());
445 assertNull(calibrator.getEstimatedMxz());
446 assertNull(calibrator.getEstimatedMyx());
447 assertNull(calibrator.getEstimatedMyz());
448 assertNull(calibrator.getEstimatedMzx());
449 assertNull(calibrator.getEstimatedMzy());
450 assertNull(calibrator.getEstimatedCovariance());
451 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
452 assertNull(calibrator.getGroundTruthGravityNorm());
453 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
454 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
455 }
456
457 @Test
458 public void testConstructor5() throws WrongSizeException {
459 final KnownPositionAccelerometerCalibrator calibrator =
460 new KnownPositionAccelerometerCalibrator(true);
461
462
463 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
464 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
465 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
466 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
467 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
468 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
469 final Acceleration bx2 = new Acceleration(0.0,
470 AccelerationUnit.FEET_PER_SQUARED_SECOND);
471 calibrator.getInitialBiasXAsAcceleration(bx2);
472 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
473 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
474 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
475 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
476 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
477 final Acceleration by2 = new Acceleration(0.0,
478 AccelerationUnit.FEET_PER_SQUARED_SECOND);
479 calibrator.getInitialBiasYAsAcceleration(by2);
480 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
481 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
482 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
483 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
484 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
485 final Acceleration bz2 = new Acceleration(0.0,
486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
487 calibrator.getInitialBiasZAsAcceleration(bz2);
488 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
489 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
490 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
491 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
492 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
493 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
494 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
495 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
496 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
497 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
498 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
499 final double[] bias1 = calibrator.getInitialBias();
500 assertArrayEquals(bias1, new double[3], 0.0);
501 final double[] bias2 = new double[3];
502 calibrator.getInitialBias(bias2);
503 assertArrayEquals(bias1, bias2, 0.0);
504 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
505 assertEquals(b1, new Matrix(3, 1));
506 final Matrix b2 = new Matrix(3, 1);
507 calibrator.getInitialBiasAsMatrix(b2);
508 assertEquals(b1, b2);
509 final Matrix ma1 = calibrator.getInitialMa();
510 assertEquals(ma1, new Matrix(3, 3));
511 final Matrix ma2 = new Matrix(3, 3);
512 calibrator.getInitialMa(ma2);
513 assertEquals(ma1, ma2);
514 assertNull(calibrator.getEcefPosition());
515 assertNull(calibrator.getNedPosition());
516 assertFalse(calibrator.getNedPosition(null));
517 assertNull(calibrator.getMeasurements());
518 assertTrue(calibrator.isCommonAxisUsed());
519 assertNull(calibrator.getListener());
520 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
521 assertFalse(calibrator.isReady());
522 assertFalse(calibrator.isRunning());
523 assertNull(calibrator.getEstimatedBiases());
524 assertFalse(calibrator.getEstimatedBiases(null));
525 assertNull(calibrator.getEstimatedBiasesAsMatrix());
526 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
527 assertNull(calibrator.getEstimatedBiasFx());
528 assertNull(calibrator.getEstimatedBiasFy());
529 assertNull(calibrator.getEstimatedBiasFz());
530 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
531 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
532 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
533 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
534 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
535 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
536 assertNull(calibrator.getEstimatedMa());
537 assertNull(calibrator.getEstimatedSx());
538 assertNull(calibrator.getEstimatedSy());
539 assertNull(calibrator.getEstimatedSz());
540 assertNull(calibrator.getEstimatedMxy());
541 assertNull(calibrator.getEstimatedMxz());
542 assertNull(calibrator.getEstimatedMyx());
543 assertNull(calibrator.getEstimatedMyz());
544 assertNull(calibrator.getEstimatedMzx());
545 assertNull(calibrator.getEstimatedMzy());
546 assertNull(calibrator.getEstimatedCovariance());
547 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
548 assertNull(calibrator.getGroundTruthGravityNorm());
549 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
550 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
551 }
552
553 @Test
554 public void testConstructor6() throws WrongSizeException {
555 final KnownPositionAccelerometerCalibrator calibrator =
556 new KnownPositionAccelerometerCalibrator(true,
557 this);
558
559
560 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
561 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
562 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
563 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
564 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
565 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
566 final Acceleration bx2 = new Acceleration(0.0,
567 AccelerationUnit.FEET_PER_SQUARED_SECOND);
568 calibrator.getInitialBiasXAsAcceleration(bx2);
569 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
570 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
571 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
572 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
573 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
574 final Acceleration by2 = new Acceleration(0.0,
575 AccelerationUnit.FEET_PER_SQUARED_SECOND);
576 calibrator.getInitialBiasYAsAcceleration(by2);
577 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
578 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
579 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
580 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
581 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
582 final Acceleration bz2 = new Acceleration(0.0,
583 AccelerationUnit.FEET_PER_SQUARED_SECOND);
584 calibrator.getInitialBiasZAsAcceleration(bz2);
585 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
586 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
587 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
588 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
589 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
590 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
591 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
592 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
593 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
594 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
595 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
596 final double[] bias1 = calibrator.getInitialBias();
597 assertArrayEquals(bias1, new double[3], 0.0);
598 final double[] bias2 = new double[3];
599 calibrator.getInitialBias(bias2);
600 assertArrayEquals(bias1, bias2, 0.0);
601 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
602 assertEquals(b1, new Matrix(3, 1));
603 final Matrix b2 = new Matrix(3, 1);
604 calibrator.getInitialBiasAsMatrix(b2);
605 assertEquals(b1, b2);
606 final Matrix ma1 = calibrator.getInitialMa();
607 assertEquals(ma1, new Matrix(3, 3));
608 final Matrix ma2 = new Matrix(3, 3);
609 calibrator.getInitialMa(ma2);
610 assertEquals(ma1, ma2);
611 assertNull(calibrator.getEcefPosition());
612 assertNull(calibrator.getNedPosition());
613 assertFalse(calibrator.getNedPosition(null));
614 assertNull(calibrator.getMeasurements());
615 assertTrue(calibrator.isCommonAxisUsed());
616 assertSame(calibrator.getListener(), this);
617 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
618 assertFalse(calibrator.isReady());
619 assertFalse(calibrator.isRunning());
620 assertNull(calibrator.getEstimatedBiases());
621 assertFalse(calibrator.getEstimatedBiases(null));
622 assertNull(calibrator.getEstimatedBiasesAsMatrix());
623 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
624 assertNull(calibrator.getEstimatedBiasFx());
625 assertNull(calibrator.getEstimatedBiasFy());
626 assertNull(calibrator.getEstimatedBiasFz());
627 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
628 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
629 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
630 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
631 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
632 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
633 assertNull(calibrator.getEstimatedMa());
634 assertNull(calibrator.getEstimatedSx());
635 assertNull(calibrator.getEstimatedSy());
636 assertNull(calibrator.getEstimatedSz());
637 assertNull(calibrator.getEstimatedMxy());
638 assertNull(calibrator.getEstimatedMxz());
639 assertNull(calibrator.getEstimatedMyx());
640 assertNull(calibrator.getEstimatedMyz());
641 assertNull(calibrator.getEstimatedMzx());
642 assertNull(calibrator.getEstimatedMzy());
643 assertNull(calibrator.getEstimatedCovariance());
644 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
645 assertNull(calibrator.getGroundTruthGravityNorm());
646 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
647 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
648 }
649
650 @Test
651 public void testConstructor7() throws WrongSizeException {
652 final Collection<StandardDeviationBodyKinematics> measurements =
653 Collections.emptyList();
654 final KnownPositionAccelerometerCalibrator calibrator =
655 new KnownPositionAccelerometerCalibrator(measurements,
656 true);
657
658
659 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
660 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
661 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
662 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
663 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
664 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
665 final Acceleration bx2 = new Acceleration(0.0,
666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
667 calibrator.getInitialBiasXAsAcceleration(bx2);
668 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
669 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
670 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
671 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
672 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
673 final Acceleration by2 = new Acceleration(0.0,
674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
675 calibrator.getInitialBiasYAsAcceleration(by2);
676 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
677 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
678 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
679 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
680 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
681 final Acceleration bz2 = new Acceleration(0.0,
682 AccelerationUnit.FEET_PER_SQUARED_SECOND);
683 calibrator.getInitialBiasZAsAcceleration(bz2);
684 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
685 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
686 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
687 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
688 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
689 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
690 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
691 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
692 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
693 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
694 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
695 final double[] bias1 = calibrator.getInitialBias();
696 assertArrayEquals(bias1, new double[3], 0.0);
697 final double[] bias2 = new double[3];
698 calibrator.getInitialBias(bias2);
699 assertArrayEquals(bias1, bias2, 0.0);
700 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
701 assertEquals(b1, new Matrix(3, 1));
702 final Matrix b2 = new Matrix(3, 1);
703 calibrator.getInitialBiasAsMatrix(b2);
704 assertEquals(b1, b2);
705 final Matrix ma1 = calibrator.getInitialMa();
706 assertEquals(ma1, new Matrix(3, 3));
707 final Matrix ma2 = new Matrix(3, 3);
708 calibrator.getInitialMa(ma2);
709 assertEquals(ma1, ma2);
710 assertNull(calibrator.getEcefPosition());
711 assertNull(calibrator.getNedPosition());
712 assertFalse(calibrator.getNedPosition(null));
713 assertSame(calibrator.getMeasurements(), measurements);
714 assertTrue(calibrator.isCommonAxisUsed());
715 assertNull(calibrator.getListener());
716 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
717 assertFalse(calibrator.isReady());
718 assertFalse(calibrator.isRunning());
719 assertNull(calibrator.getEstimatedBiases());
720 assertFalse(calibrator.getEstimatedBiases(null));
721 assertNull(calibrator.getEstimatedBiasesAsMatrix());
722 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
723 assertNull(calibrator.getEstimatedBiasFx());
724 assertNull(calibrator.getEstimatedBiasFy());
725 assertNull(calibrator.getEstimatedBiasFz());
726 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
727 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
728 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
729 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
730 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
731 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
732 assertNull(calibrator.getEstimatedMa());
733 assertNull(calibrator.getEstimatedSx());
734 assertNull(calibrator.getEstimatedSy());
735 assertNull(calibrator.getEstimatedSz());
736 assertNull(calibrator.getEstimatedMxy());
737 assertNull(calibrator.getEstimatedMxz());
738 assertNull(calibrator.getEstimatedMyx());
739 assertNull(calibrator.getEstimatedMyz());
740 assertNull(calibrator.getEstimatedMzx());
741 assertNull(calibrator.getEstimatedMzy());
742 assertNull(calibrator.getEstimatedCovariance());
743 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
744 assertNull(calibrator.getGroundTruthGravityNorm());
745 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
746 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
747 }
748
749 @Test
750 public void testConstructor8() throws WrongSizeException {
751 final Collection<StandardDeviationBodyKinematics> measurements =
752 Collections.emptyList();
753 final KnownPositionAccelerometerCalibrator calibrator =
754 new KnownPositionAccelerometerCalibrator(measurements,
755 true, this);
756
757
758 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
759 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
760 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
761 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
762 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
763 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
764 final Acceleration bx2 = new Acceleration(0.0,
765 AccelerationUnit.FEET_PER_SQUARED_SECOND);
766 calibrator.getInitialBiasXAsAcceleration(bx2);
767 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
768 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
769 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
770 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
771 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
772 final Acceleration by2 = new Acceleration(0.0,
773 AccelerationUnit.FEET_PER_SQUARED_SECOND);
774 calibrator.getInitialBiasYAsAcceleration(by2);
775 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
776 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
777 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
778 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
779 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
780 final Acceleration bz2 = new Acceleration(0.0,
781 AccelerationUnit.FEET_PER_SQUARED_SECOND);
782 calibrator.getInitialBiasZAsAcceleration(bz2);
783 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
784 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
785 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
786 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
787 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
788 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
789 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
790 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
791 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
792 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
793 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
794 final double[] bias1 = calibrator.getInitialBias();
795 assertArrayEquals(bias1, new double[3], 0.0);
796 final double[] bias2 = new double[3];
797 calibrator.getInitialBias(bias2);
798 assertArrayEquals(bias1, bias2, 0.0);
799 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
800 assertEquals(b1, new Matrix(3, 1));
801 final Matrix b2 = new Matrix(3, 1);
802 calibrator.getInitialBiasAsMatrix(b2);
803 assertEquals(b1, b2);
804 final Matrix ma1 = calibrator.getInitialMa();
805 assertEquals(ma1, new Matrix(3, 3));
806 final Matrix ma2 = new Matrix(3, 3);
807 calibrator.getInitialMa(ma2);
808 assertEquals(ma1, ma2);
809 assertNull(calibrator.getEcefPosition());
810 assertNull(calibrator.getNedPosition());
811 assertFalse(calibrator.getNedPosition(null));
812 assertSame(calibrator.getMeasurements(), measurements);
813 assertTrue(calibrator.isCommonAxisUsed());
814 assertSame(calibrator.getListener(), this);
815 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
816 assertFalse(calibrator.isReady());
817 assertFalse(calibrator.isRunning());
818 assertNull(calibrator.getEstimatedBiases());
819 assertFalse(calibrator.getEstimatedBiases(null));
820 assertNull(calibrator.getEstimatedBiasesAsMatrix());
821 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
822 assertNull(calibrator.getEstimatedBiasFx());
823 assertNull(calibrator.getEstimatedBiasFy());
824 assertNull(calibrator.getEstimatedBiasFz());
825 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
826 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
827 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
828 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
829 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
830 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
831 assertNull(calibrator.getEstimatedMa());
832 assertNull(calibrator.getEstimatedSx());
833 assertNull(calibrator.getEstimatedSy());
834 assertNull(calibrator.getEstimatedSz());
835 assertNull(calibrator.getEstimatedMxy());
836 assertNull(calibrator.getEstimatedMxz());
837 assertNull(calibrator.getEstimatedMyx());
838 assertNull(calibrator.getEstimatedMyz());
839 assertNull(calibrator.getEstimatedMzx());
840 assertNull(calibrator.getEstimatedMzy());
841 assertNull(calibrator.getEstimatedCovariance());
842 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
843 assertNull(calibrator.getGroundTruthGravityNorm());
844 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
845 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
846 }
847
848 @Test
849 public void testConstructor9() throws WrongSizeException {
850 final Matrix ba = generateBa();
851 final double biasX = ba.getElementAtIndex(0);
852 final double biasY = ba.getElementAtIndex(1);
853 final double biasZ = ba.getElementAtIndex(2);
854
855 final KnownPositionAccelerometerCalibrator calibrator =
856 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ);
857
858
859 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
860 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
861 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
862 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
863 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
864 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
865 final Acceleration bx2 = new Acceleration(0.0,
866 AccelerationUnit.FEET_PER_SQUARED_SECOND);
867 calibrator.getInitialBiasXAsAcceleration(bx2);
868 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
869 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
870 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
871 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
872 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
873 final Acceleration by2 = new Acceleration(0.0,
874 AccelerationUnit.FEET_PER_SQUARED_SECOND);
875 calibrator.getInitialBiasYAsAcceleration(by2);
876 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
877 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
878 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
879 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
880 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
881 final Acceleration bz2 = new Acceleration(0.0,
882 AccelerationUnit.FEET_PER_SQUARED_SECOND);
883 calibrator.getInitialBiasZAsAcceleration(bz2);
884 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
885 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
886 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
887 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
888 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
889 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
890 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
891 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
892 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
893 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
894 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
895 final double[] bias1 = calibrator.getInitialBias();
896 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
897 final double[] bias2 = new double[3];
898 calibrator.getInitialBias(bias2);
899 assertArrayEquals(bias1, bias2, 0.0);
900 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
901 assertEquals(b1, ba);
902 final Matrix b2 = new Matrix(3, 1);
903 calibrator.getInitialBiasAsMatrix(b2);
904 assertEquals(b1, b2);
905 final Matrix ma1 = calibrator.getInitialMa();
906 assertEquals(ma1, new Matrix(3, 3));
907 final Matrix ma2 = new Matrix(3, 3);
908 calibrator.getInitialMa(ma2);
909 assertEquals(ma1, ma2);
910 assertNull(calibrator.getEcefPosition());
911 assertNull(calibrator.getNedPosition());
912 assertFalse(calibrator.getNedPosition(null));
913 assertNull(calibrator.getMeasurements());
914 assertFalse(calibrator.isCommonAxisUsed());
915 assertNull(calibrator.getListener());
916 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
917 assertFalse(calibrator.isReady());
918 assertFalse(calibrator.isRunning());
919 assertNull(calibrator.getEstimatedBiases());
920 assertFalse(calibrator.getEstimatedBiases(null));
921 assertNull(calibrator.getEstimatedBiasesAsMatrix());
922 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
923 assertNull(calibrator.getEstimatedBiasFx());
924 assertNull(calibrator.getEstimatedBiasFy());
925 assertNull(calibrator.getEstimatedBiasFz());
926 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
927 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
928 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
929 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
930 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
931 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
932 assertNull(calibrator.getEstimatedMa());
933 assertNull(calibrator.getEstimatedSx());
934 assertNull(calibrator.getEstimatedSy());
935 assertNull(calibrator.getEstimatedSz());
936 assertNull(calibrator.getEstimatedMxy());
937 assertNull(calibrator.getEstimatedMxz());
938 assertNull(calibrator.getEstimatedMyx());
939 assertNull(calibrator.getEstimatedMyz());
940 assertNull(calibrator.getEstimatedMzx());
941 assertNull(calibrator.getEstimatedMzy());
942 assertNull(calibrator.getEstimatedCovariance());
943 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
944 assertNull(calibrator.getGroundTruthGravityNorm());
945 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
946 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
947 }
948
949 @Test
950 public void testConstructor10() throws WrongSizeException {
951 final Matrix ba = generateBa();
952 final double biasX = ba.getElementAtIndex(0);
953 final double biasY = ba.getElementAtIndex(1);
954 final double biasZ = ba.getElementAtIndex(2);
955
956 final KnownPositionAccelerometerCalibrator calibrator =
957 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
958 this);
959
960
961 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
962 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
963 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
964 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
965 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
966 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
967 final Acceleration bx2 = new Acceleration(0.0,
968 AccelerationUnit.FEET_PER_SQUARED_SECOND);
969 calibrator.getInitialBiasXAsAcceleration(bx2);
970 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
971 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
972 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
973 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
974 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
975 final Acceleration by2 = new Acceleration(0.0,
976 AccelerationUnit.FEET_PER_SQUARED_SECOND);
977 calibrator.getInitialBiasYAsAcceleration(by2);
978 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
979 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
980 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
981 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
982 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
983 final Acceleration bz2 = new Acceleration(0.0,
984 AccelerationUnit.FEET_PER_SQUARED_SECOND);
985 calibrator.getInitialBiasZAsAcceleration(bz2);
986 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
987 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
988 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
989 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
990 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
991 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
992 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
993 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
994 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
995 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
996 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
997 final double[] bias1 = calibrator.getInitialBias();
998 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
999 final double[] bias2 = new double[3];
1000 calibrator.getInitialBias(bias2);
1001 assertArrayEquals(bias1, bias2, 0.0);
1002 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1003 assertEquals(b1, ba);
1004 final Matrix b2 = new Matrix(3, 1);
1005 calibrator.getInitialBiasAsMatrix(b2);
1006 assertEquals(b1, b2);
1007 final Matrix ma1 = calibrator.getInitialMa();
1008 assertEquals(ma1, new Matrix(3, 3));
1009 final Matrix ma2 = new Matrix(3, 3);
1010 calibrator.getInitialMa(ma2);
1011 assertEquals(ma1, ma2);
1012 assertNull(calibrator.getEcefPosition());
1013 assertNull(calibrator.getNedPosition());
1014 assertFalse(calibrator.getNedPosition(null));
1015 assertNull(calibrator.getMeasurements());
1016 assertFalse(calibrator.isCommonAxisUsed());
1017 assertSame(calibrator.getListener(), this);
1018 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1019 assertFalse(calibrator.isReady());
1020 assertFalse(calibrator.isRunning());
1021 assertNull(calibrator.getEstimatedBiases());
1022 assertFalse(calibrator.getEstimatedBiases(null));
1023 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1024 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1025 assertNull(calibrator.getEstimatedBiasFx());
1026 assertNull(calibrator.getEstimatedBiasFy());
1027 assertNull(calibrator.getEstimatedBiasFz());
1028 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1029 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1030 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1031 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1032 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1033 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1034 assertNull(calibrator.getEstimatedMa());
1035 assertNull(calibrator.getEstimatedSx());
1036 assertNull(calibrator.getEstimatedSy());
1037 assertNull(calibrator.getEstimatedSz());
1038 assertNull(calibrator.getEstimatedMxy());
1039 assertNull(calibrator.getEstimatedMxz());
1040 assertNull(calibrator.getEstimatedMyx());
1041 assertNull(calibrator.getEstimatedMyz());
1042 assertNull(calibrator.getEstimatedMzx());
1043 assertNull(calibrator.getEstimatedMzy());
1044 assertNull(calibrator.getEstimatedCovariance());
1045 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1046 assertNull(calibrator.getGroundTruthGravityNorm());
1047 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1048 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1049 }
1050
1051 @Test
1052 public void testConstructor11() throws WrongSizeException {
1053 final Collection<StandardDeviationBodyKinematics> measurements =
1054 Collections.emptyList();
1055
1056 final Matrix ba = generateBa();
1057 final double biasX = ba.getElementAtIndex(0);
1058 final double biasY = ba.getElementAtIndex(1);
1059 final double biasZ = ba.getElementAtIndex(2);
1060
1061 final KnownPositionAccelerometerCalibrator calibrator =
1062 new KnownPositionAccelerometerCalibrator(measurements,
1063 biasX, biasY, biasZ);
1064
1065
1066 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1067 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1068 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1069 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1070 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1071 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1072 final Acceleration bx2 = new Acceleration(0.0,
1073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1074 calibrator.getInitialBiasXAsAcceleration(bx2);
1075 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1076 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1077 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1078 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1079 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1080 final Acceleration by2 = new Acceleration(0.0,
1081 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1082 calibrator.getInitialBiasYAsAcceleration(by2);
1083 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1084 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1085 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1086 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1087 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1088 final Acceleration bz2 = new Acceleration(0.0,
1089 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1090 calibrator.getInitialBiasZAsAcceleration(bz2);
1091 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1092 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1093 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1094 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1095 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1096 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1097 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1098 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1099 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1100 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1101 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1102 final double[] bias1 = calibrator.getInitialBias();
1103 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1104 final double[] bias2 = new double[3];
1105 calibrator.getInitialBias(bias2);
1106 assertArrayEquals(bias1, bias2, 0.0);
1107 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1108 assertEquals(b1, ba);
1109 final Matrix b2 = new Matrix(3, 1);
1110 calibrator.getInitialBiasAsMatrix(b2);
1111 assertEquals(b1, b2);
1112 final Matrix ma1 = calibrator.getInitialMa();
1113 assertEquals(ma1, new Matrix(3, 3));
1114 final Matrix ma2 = new Matrix(3, 3);
1115 calibrator.getInitialMa(ma2);
1116 assertEquals(ma1, ma2);
1117 assertNull(calibrator.getEcefPosition());
1118 assertNull(calibrator.getNedPosition());
1119 assertFalse(calibrator.getNedPosition(null));
1120 assertSame(calibrator.getMeasurements(), measurements);
1121 assertFalse(calibrator.isCommonAxisUsed());
1122 assertNull(calibrator.getListener());
1123 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1124 assertFalse(calibrator.isReady());
1125 assertFalse(calibrator.isRunning());
1126 assertNull(calibrator.getEstimatedBiases());
1127 assertFalse(calibrator.getEstimatedBiases(null));
1128 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1129 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1130 assertNull(calibrator.getEstimatedBiasFx());
1131 assertNull(calibrator.getEstimatedBiasFy());
1132 assertNull(calibrator.getEstimatedBiasFz());
1133 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1134 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1135 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1136 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1137 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1138 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1139 assertNull(calibrator.getEstimatedMa());
1140 assertNull(calibrator.getEstimatedSx());
1141 assertNull(calibrator.getEstimatedSy());
1142 assertNull(calibrator.getEstimatedSz());
1143 assertNull(calibrator.getEstimatedMxy());
1144 assertNull(calibrator.getEstimatedMxz());
1145 assertNull(calibrator.getEstimatedMyx());
1146 assertNull(calibrator.getEstimatedMyz());
1147 assertNull(calibrator.getEstimatedMzx());
1148 assertNull(calibrator.getEstimatedMzy());
1149 assertNull(calibrator.getEstimatedCovariance());
1150 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1151 assertNull(calibrator.getGroundTruthGravityNorm());
1152 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1153 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1154 }
1155
1156 @Test
1157 public void testConstructor12() throws WrongSizeException {
1158 final Collection<StandardDeviationBodyKinematics> measurements =
1159 Collections.emptyList();
1160
1161 final Matrix ba = generateBa();
1162 final double biasX = ba.getElementAtIndex(0);
1163 final double biasY = ba.getElementAtIndex(1);
1164 final double biasZ = ba.getElementAtIndex(2);
1165
1166 final KnownPositionAccelerometerCalibrator calibrator =
1167 new KnownPositionAccelerometerCalibrator(measurements,
1168 biasX, biasY, biasZ, this);
1169
1170
1171 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1172 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1173 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1174 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1175 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1176 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1177 final Acceleration bx2 = new Acceleration(0.0,
1178 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1179 calibrator.getInitialBiasXAsAcceleration(bx2);
1180 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1181 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1182 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1183 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1184 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1185 final Acceleration by2 = new Acceleration(0.0,
1186 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1187 calibrator.getInitialBiasYAsAcceleration(by2);
1188 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1189 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1190 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1191 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1192 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1193 final Acceleration bz2 = new Acceleration(0.0,
1194 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1195 calibrator.getInitialBiasZAsAcceleration(bz2);
1196 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1197 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1198 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1199 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1200 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1201 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1202 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1203 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1204 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1205 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1206 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1207 final double[] bias1 = calibrator.getInitialBias();
1208 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1209 final double[] bias2 = new double[3];
1210 calibrator.getInitialBias(bias2);
1211 assertArrayEquals(bias1, bias2, 0.0);
1212 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1213 assertEquals(b1, ba);
1214 final Matrix b2 = new Matrix(3, 1);
1215 calibrator.getInitialBiasAsMatrix(b2);
1216 assertEquals(b1, b2);
1217 final Matrix ma1 = calibrator.getInitialMa();
1218 assertEquals(ma1, new Matrix(3, 3));
1219 final Matrix ma2 = new Matrix(3, 3);
1220 calibrator.getInitialMa(ma2);
1221 assertEquals(ma1, ma2);
1222 assertNull(calibrator.getEcefPosition());
1223 assertNull(calibrator.getNedPosition());
1224 assertFalse(calibrator.getNedPosition(null));
1225 assertSame(calibrator.getMeasurements(), measurements);
1226 assertFalse(calibrator.isCommonAxisUsed());
1227 assertSame(calibrator.getListener(), this);
1228 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1229 assertFalse(calibrator.isReady());
1230 assertFalse(calibrator.isRunning());
1231 assertNull(calibrator.getEstimatedBiases());
1232 assertFalse(calibrator.getEstimatedBiases(null));
1233 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1234 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1235 assertNull(calibrator.getEstimatedBiasFx());
1236 assertNull(calibrator.getEstimatedBiasFy());
1237 assertNull(calibrator.getEstimatedBiasFz());
1238 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1239 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1240 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1241 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1242 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1243 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1244 assertNull(calibrator.getEstimatedMa());
1245 assertNull(calibrator.getEstimatedSx());
1246 assertNull(calibrator.getEstimatedSy());
1247 assertNull(calibrator.getEstimatedSz());
1248 assertNull(calibrator.getEstimatedMxy());
1249 assertNull(calibrator.getEstimatedMxz());
1250 assertNull(calibrator.getEstimatedMyx());
1251 assertNull(calibrator.getEstimatedMyz());
1252 assertNull(calibrator.getEstimatedMzx());
1253 assertNull(calibrator.getEstimatedMzy());
1254 assertNull(calibrator.getEstimatedCovariance());
1255 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1256 assertNull(calibrator.getGroundTruthGravityNorm());
1257 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1258 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1259 }
1260
1261 @Test
1262 public void testConstructor13() throws WrongSizeException {
1263 final Matrix ba = generateBa();
1264 final double biasX = ba.getElementAtIndex(0);
1265 final double biasY = ba.getElementAtIndex(1);
1266 final double biasZ = ba.getElementAtIndex(2);
1267
1268 final KnownPositionAccelerometerCalibrator calibrator =
1269 new KnownPositionAccelerometerCalibrator(true,
1270 biasX, biasY, biasZ);
1271
1272
1273 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1274 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1275 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1276 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1277 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1278 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1279 final Acceleration bx2 = new Acceleration(0.0,
1280 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1281 calibrator.getInitialBiasXAsAcceleration(bx2);
1282 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1283 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1284 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1285 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1286 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1287 final Acceleration by2 = new Acceleration(0.0,
1288 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1289 calibrator.getInitialBiasYAsAcceleration(by2);
1290 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1291 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1292 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1293 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1294 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1295 final Acceleration bz2 = new Acceleration(0.0,
1296 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1297 calibrator.getInitialBiasZAsAcceleration(bz2);
1298 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1299 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1300 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1301 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1302 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1303 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1304 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1305 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1306 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1307 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1308 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1309 final double[] bias1 = calibrator.getInitialBias();
1310 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1311 final double[] bias2 = new double[3];
1312 calibrator.getInitialBias(bias2);
1313 assertArrayEquals(bias1, bias2, 0.0);
1314 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1315 assertEquals(b1, ba);
1316 final Matrix b2 = new Matrix(3, 1);
1317 calibrator.getInitialBiasAsMatrix(b2);
1318 assertEquals(b1, b2);
1319 final Matrix ma1 = calibrator.getInitialMa();
1320 assertEquals(ma1, new Matrix(3, 3));
1321 final Matrix ma2 = new Matrix(3, 3);
1322 calibrator.getInitialMa(ma2);
1323 assertEquals(ma1, ma2);
1324 assertNull(calibrator.getEcefPosition());
1325 assertNull(calibrator.getNedPosition());
1326 assertFalse(calibrator.getNedPosition(null));
1327 assertNull(calibrator.getMeasurements());
1328 assertTrue(calibrator.isCommonAxisUsed());
1329 assertNull(calibrator.getListener());
1330 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1331 assertFalse(calibrator.isReady());
1332 assertFalse(calibrator.isRunning());
1333 assertNull(calibrator.getEstimatedBiases());
1334 assertFalse(calibrator.getEstimatedBiases(null));
1335 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1336 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1337 assertNull(calibrator.getEstimatedBiasFx());
1338 assertNull(calibrator.getEstimatedBiasFy());
1339 assertNull(calibrator.getEstimatedBiasFz());
1340 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1341 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1342 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1343 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1344 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1345 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1346 assertNull(calibrator.getEstimatedMa());
1347 assertNull(calibrator.getEstimatedSx());
1348 assertNull(calibrator.getEstimatedSy());
1349 assertNull(calibrator.getEstimatedSz());
1350 assertNull(calibrator.getEstimatedMxy());
1351 assertNull(calibrator.getEstimatedMxz());
1352 assertNull(calibrator.getEstimatedMyx());
1353 assertNull(calibrator.getEstimatedMyz());
1354 assertNull(calibrator.getEstimatedMzx());
1355 assertNull(calibrator.getEstimatedMzy());
1356 assertNull(calibrator.getEstimatedCovariance());
1357 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1358 assertNull(calibrator.getGroundTruthGravityNorm());
1359 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1360 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1361 }
1362
1363 @Test
1364 public void testConstructor14() throws WrongSizeException {
1365 final Matrix ba = generateBa();
1366 final double biasX = ba.getElementAtIndex(0);
1367 final double biasY = ba.getElementAtIndex(1);
1368 final double biasZ = ba.getElementAtIndex(2);
1369
1370 final KnownPositionAccelerometerCalibrator calibrator =
1371 new KnownPositionAccelerometerCalibrator(true,
1372 biasX, biasY, biasZ, this);
1373
1374
1375 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1376 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1377 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1378 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1379 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1380 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1381 final Acceleration bx2 = new Acceleration(0.0,
1382 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1383 calibrator.getInitialBiasXAsAcceleration(bx2);
1384 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1385 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1386 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1387 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1388 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1389 final Acceleration by2 = new Acceleration(0.0,
1390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1391 calibrator.getInitialBiasYAsAcceleration(by2);
1392 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1393 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1394 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1395 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1396 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1397 final Acceleration bz2 = new Acceleration(0.0,
1398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1399 calibrator.getInitialBiasZAsAcceleration(bz2);
1400 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1401 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1402 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1403 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1404 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1405 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1406 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1407 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1408 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1409 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1410 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1411 final double[] bias1 = calibrator.getInitialBias();
1412 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1413 final double[] bias2 = new double[3];
1414 calibrator.getInitialBias(bias2);
1415 assertArrayEquals(bias1, bias2, 0.0);
1416 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1417 assertEquals(b1, ba);
1418 final Matrix b2 = new Matrix(3, 1);
1419 calibrator.getInitialBiasAsMatrix(b2);
1420 assertEquals(b1, b2);
1421 final Matrix ma1 = calibrator.getInitialMa();
1422 assertEquals(ma1, new Matrix(3, 3));
1423 final Matrix ma2 = new Matrix(3, 3);
1424 calibrator.getInitialMa(ma2);
1425 assertEquals(ma1, ma2);
1426 assertNull(calibrator.getEcefPosition());
1427 assertNull(calibrator.getNedPosition());
1428 assertFalse(calibrator.getNedPosition(null));
1429 assertNull(calibrator.getMeasurements());
1430 assertTrue(calibrator.isCommonAxisUsed());
1431 assertSame(calibrator.getListener(), this);
1432 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1433 assertFalse(calibrator.isReady());
1434 assertFalse(calibrator.isRunning());
1435 assertNull(calibrator.getEstimatedBiases());
1436 assertFalse(calibrator.getEstimatedBiases(null));
1437 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1438 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1439 assertNull(calibrator.getEstimatedBiasFx());
1440 assertNull(calibrator.getEstimatedBiasFy());
1441 assertNull(calibrator.getEstimatedBiasFz());
1442 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1443 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1444 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1445 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1446 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1447 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1448 assertNull(calibrator.getEstimatedMa());
1449 assertNull(calibrator.getEstimatedSx());
1450 assertNull(calibrator.getEstimatedSy());
1451 assertNull(calibrator.getEstimatedSz());
1452 assertNull(calibrator.getEstimatedMxy());
1453 assertNull(calibrator.getEstimatedMxz());
1454 assertNull(calibrator.getEstimatedMyx());
1455 assertNull(calibrator.getEstimatedMyz());
1456 assertNull(calibrator.getEstimatedMzx());
1457 assertNull(calibrator.getEstimatedMzy());
1458 assertNull(calibrator.getEstimatedCovariance());
1459 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1460 assertNull(calibrator.getGroundTruthGravityNorm());
1461 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1462 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1463 }
1464
1465 @Test
1466 public void testConstructor15() throws WrongSizeException {
1467 final Collection<StandardDeviationBodyKinematics> measurements =
1468 Collections.emptyList();
1469
1470 final Matrix ba = generateBa();
1471 final double biasX = ba.getElementAtIndex(0);
1472 final double biasY = ba.getElementAtIndex(1);
1473 final double biasZ = ba.getElementAtIndex(2);
1474
1475 final KnownPositionAccelerometerCalibrator calibrator =
1476 new KnownPositionAccelerometerCalibrator(measurements,
1477 true, biasX, biasY, biasZ);
1478
1479
1480 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1481 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1482 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1483 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1484 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1485 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1486 final Acceleration bx2 = new Acceleration(0.0,
1487 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1488 calibrator.getInitialBiasXAsAcceleration(bx2);
1489 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1490 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1491 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1492 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1493 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1494 final Acceleration by2 = new Acceleration(0.0,
1495 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1496 calibrator.getInitialBiasYAsAcceleration(by2);
1497 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1498 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1499 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1500 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1501 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1502 final Acceleration bz2 = new Acceleration(0.0,
1503 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1504 calibrator.getInitialBiasZAsAcceleration(bz2);
1505 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1506 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1507 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1508 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1509 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1510 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1511 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1512 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1513 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1514 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1515 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1516 final double[] bias1 = calibrator.getInitialBias();
1517 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1518 final double[] bias2 = new double[3];
1519 calibrator.getInitialBias(bias2);
1520 assertArrayEquals(bias1, bias2, 0.0);
1521 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1522 assertEquals(b1, ba);
1523 final Matrix b2 = new Matrix(3, 1);
1524 calibrator.getInitialBiasAsMatrix(b2);
1525 assertEquals(b1, b2);
1526 final Matrix ma1 = calibrator.getInitialMa();
1527 assertEquals(ma1, new Matrix(3, 3));
1528 final Matrix ma2 = new Matrix(3, 3);
1529 calibrator.getInitialMa(ma2);
1530 assertEquals(ma1, ma2);
1531 assertNull(calibrator.getEcefPosition());
1532 assertNull(calibrator.getNedPosition());
1533 assertFalse(calibrator.getNedPosition(null));
1534 assertSame(calibrator.getMeasurements(), measurements);
1535 assertTrue(calibrator.isCommonAxisUsed());
1536 assertNull(calibrator.getListener());
1537 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1538 assertFalse(calibrator.isReady());
1539 assertFalse(calibrator.isRunning());
1540 assertNull(calibrator.getEstimatedBiases());
1541 assertFalse(calibrator.getEstimatedBiases(null));
1542 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1543 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1544 assertNull(calibrator.getEstimatedBiasFx());
1545 assertNull(calibrator.getEstimatedBiasFy());
1546 assertNull(calibrator.getEstimatedBiasFz());
1547 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1548 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1549 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1550 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1551 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1552 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1553 assertNull(calibrator.getEstimatedMa());
1554 assertNull(calibrator.getEstimatedSx());
1555 assertNull(calibrator.getEstimatedSy());
1556 assertNull(calibrator.getEstimatedSz());
1557 assertNull(calibrator.getEstimatedMxy());
1558 assertNull(calibrator.getEstimatedMxz());
1559 assertNull(calibrator.getEstimatedMyx());
1560 assertNull(calibrator.getEstimatedMyz());
1561 assertNull(calibrator.getEstimatedMzx());
1562 assertNull(calibrator.getEstimatedMzy());
1563 assertNull(calibrator.getEstimatedCovariance());
1564 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1565 assertNull(calibrator.getGroundTruthGravityNorm());
1566 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1567 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1568 }
1569
1570 @Test
1571 public void testConstructor16() throws WrongSizeException {
1572 final Collection<StandardDeviationBodyKinematics> measurements =
1573 Collections.emptyList();
1574
1575 final Matrix ba = generateBa();
1576 final double biasX = ba.getElementAtIndex(0);
1577 final double biasY = ba.getElementAtIndex(1);
1578 final double biasZ = ba.getElementAtIndex(2);
1579
1580 final KnownPositionAccelerometerCalibrator calibrator =
1581 new KnownPositionAccelerometerCalibrator(measurements,
1582 true, biasX, biasY, biasZ, this);
1583
1584
1585 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1586 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1587 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1588 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1589 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1590 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1591 final Acceleration bx2 = new Acceleration(0.0,
1592 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1593 calibrator.getInitialBiasXAsAcceleration(bx2);
1594 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1595 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1596 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1597 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1598 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1599 final Acceleration by2 = new Acceleration(0.0,
1600 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1601 calibrator.getInitialBiasYAsAcceleration(by2);
1602 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1603 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1604 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1605 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1606 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1607 final Acceleration bz2 = new Acceleration(0.0,
1608 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1609 calibrator.getInitialBiasZAsAcceleration(bz2);
1610 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1611 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1612 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1613 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1614 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1615 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1616 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1617 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1618 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1619 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1620 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1621 final double[] bias1 = calibrator.getInitialBias();
1622 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1623 final double[] bias2 = new double[3];
1624 calibrator.getInitialBias(bias2);
1625 assertArrayEquals(bias1, bias2, 0.0);
1626 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1627 assertEquals(b1, ba);
1628 final Matrix b2 = new Matrix(3, 1);
1629 calibrator.getInitialBiasAsMatrix(b2);
1630 assertEquals(b1, b2);
1631 final Matrix ma1 = calibrator.getInitialMa();
1632 assertEquals(ma1, new Matrix(3, 3));
1633 final Matrix ma2 = new Matrix(3, 3);
1634 calibrator.getInitialMa(ma2);
1635 assertEquals(ma1, ma2);
1636 assertNull(calibrator.getEcefPosition());
1637 assertNull(calibrator.getNedPosition());
1638 assertFalse(calibrator.getNedPosition(null));
1639 assertSame(calibrator.getMeasurements(), measurements);
1640 assertTrue(calibrator.isCommonAxisUsed());
1641 assertSame(calibrator.getListener(), this);
1642 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1643 assertFalse(calibrator.isReady());
1644 assertFalse(calibrator.isRunning());
1645 assertNull(calibrator.getEstimatedBiases());
1646 assertFalse(calibrator.getEstimatedBiases(null));
1647 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1648 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1649 assertNull(calibrator.getEstimatedBiasFx());
1650 assertNull(calibrator.getEstimatedBiasFy());
1651 assertNull(calibrator.getEstimatedBiasFz());
1652 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1653 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1654 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1655 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1656 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1657 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1658 assertNull(calibrator.getEstimatedMa());
1659 assertNull(calibrator.getEstimatedSx());
1660 assertNull(calibrator.getEstimatedSy());
1661 assertNull(calibrator.getEstimatedSz());
1662 assertNull(calibrator.getEstimatedMxy());
1663 assertNull(calibrator.getEstimatedMxz());
1664 assertNull(calibrator.getEstimatedMyx());
1665 assertNull(calibrator.getEstimatedMyz());
1666 assertNull(calibrator.getEstimatedMzx());
1667 assertNull(calibrator.getEstimatedMzy());
1668 assertNull(calibrator.getEstimatedCovariance());
1669 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1670 assertNull(calibrator.getGroundTruthGravityNorm());
1671 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1672 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1673 }
1674
1675 @Test
1676 public void testConstructor17() throws WrongSizeException {
1677 final Matrix ba = generateBa();
1678 final double biasX = ba.getElementAtIndex(0);
1679 final double biasY = ba.getElementAtIndex(1);
1680 final double biasZ = ba.getElementAtIndex(2);
1681
1682 final Acceleration bx = new Acceleration(biasX,
1683 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1684 final Acceleration by = new Acceleration(biasY,
1685 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1686 final Acceleration bz = new Acceleration(biasZ,
1687 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1688
1689 final KnownPositionAccelerometerCalibrator calibrator =
1690 new KnownPositionAccelerometerCalibrator(bx, by, bz);
1691
1692
1693 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1694 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1695 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1696 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1697 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1698 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1699 final Acceleration bx2 = new Acceleration(0.0,
1700 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1701 calibrator.getInitialBiasXAsAcceleration(bx2);
1702 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1703 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1704 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1705 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1706 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1707 final Acceleration by2 = new Acceleration(0.0,
1708 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1709 calibrator.getInitialBiasYAsAcceleration(by2);
1710 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1711 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1712 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1713 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1714 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1715 final Acceleration bz2 = new Acceleration(0.0,
1716 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1717 calibrator.getInitialBiasZAsAcceleration(bz2);
1718 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1719 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1720 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1721 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1722 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1723 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1724 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1725 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1726 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1727 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1728 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1729 final double[] bias1 = calibrator.getInitialBias();
1730 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1731 final double[] bias2 = new double[3];
1732 calibrator.getInitialBias(bias2);
1733 assertArrayEquals(bias1, bias2, 0.0);
1734 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1735 assertEquals(b1, ba);
1736 final Matrix b2 = new Matrix(3, 1);
1737 calibrator.getInitialBiasAsMatrix(b2);
1738 assertEquals(b1, b2);
1739 final Matrix ma1 = calibrator.getInitialMa();
1740 assertEquals(ma1, new Matrix(3, 3));
1741 final Matrix ma2 = new Matrix(3, 3);
1742 calibrator.getInitialMa(ma2);
1743 assertEquals(ma1, ma2);
1744 assertNull(calibrator.getEcefPosition());
1745 assertNull(calibrator.getNedPosition());
1746 assertFalse(calibrator.getNedPosition(null));
1747 assertNull(calibrator.getMeasurements());
1748 assertFalse(calibrator.isCommonAxisUsed());
1749 assertNull(calibrator.getListener());
1750 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1751 assertFalse(calibrator.isReady());
1752 assertFalse(calibrator.isRunning());
1753 assertNull(calibrator.getEstimatedBiases());
1754 assertFalse(calibrator.getEstimatedBiases(null));
1755 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1756 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1757 assertNull(calibrator.getEstimatedBiasFx());
1758 assertNull(calibrator.getEstimatedBiasFy());
1759 assertNull(calibrator.getEstimatedBiasFz());
1760 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1761 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1762 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1763 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1764 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1765 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1766 assertNull(calibrator.getEstimatedMa());
1767 assertNull(calibrator.getEstimatedSx());
1768 assertNull(calibrator.getEstimatedSy());
1769 assertNull(calibrator.getEstimatedSz());
1770 assertNull(calibrator.getEstimatedMxy());
1771 assertNull(calibrator.getEstimatedMxz());
1772 assertNull(calibrator.getEstimatedMyx());
1773 assertNull(calibrator.getEstimatedMyz());
1774 assertNull(calibrator.getEstimatedMzx());
1775 assertNull(calibrator.getEstimatedMzy());
1776 assertNull(calibrator.getEstimatedCovariance());
1777 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1778 assertNull(calibrator.getGroundTruthGravityNorm());
1779 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1780 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1781 }
1782
1783 @Test
1784 public void testConstructor18() throws WrongSizeException {
1785 final Matrix ba = generateBa();
1786 final double biasX = ba.getElementAtIndex(0);
1787 final double biasY = ba.getElementAtIndex(1);
1788 final double biasZ = ba.getElementAtIndex(2);
1789
1790 final Acceleration bx = new Acceleration(biasX,
1791 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1792 final Acceleration by = new Acceleration(biasY,
1793 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1794 final Acceleration bz = new Acceleration(biasZ,
1795 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1796
1797 final KnownPositionAccelerometerCalibrator calibrator =
1798 new KnownPositionAccelerometerCalibrator(bx, by, bz,
1799 this);
1800
1801
1802 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1803 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1804 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1805 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1806 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1807 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1808 final Acceleration bx2 = new Acceleration(0.0,
1809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1810 calibrator.getInitialBiasXAsAcceleration(bx2);
1811 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1812 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1813 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1814 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1815 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1816 final Acceleration by2 = new Acceleration(0.0,
1817 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1818 calibrator.getInitialBiasYAsAcceleration(by2);
1819 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1820 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1821 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1822 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1823 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1824 final Acceleration bz2 = new Acceleration(0.0,
1825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1826 calibrator.getInitialBiasZAsAcceleration(bz2);
1827 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1828 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1829 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1830 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1831 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1832 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1833 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1834 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1835 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1836 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1837 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1838 final double[] bias1 = calibrator.getInitialBias();
1839 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1840 final double[] bias2 = new double[3];
1841 calibrator.getInitialBias(bias2);
1842 assertArrayEquals(bias1, bias2, 0.0);
1843 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1844 assertEquals(b1, ba);
1845 final Matrix b2 = new Matrix(3, 1);
1846 calibrator.getInitialBiasAsMatrix(b2);
1847 assertEquals(b1, b2);
1848 final Matrix ma1 = calibrator.getInitialMa();
1849 assertEquals(ma1, new Matrix(3, 3));
1850 final Matrix ma2 = new Matrix(3, 3);
1851 calibrator.getInitialMa(ma2);
1852 assertEquals(ma1, ma2);
1853 assertNull(calibrator.getEcefPosition());
1854 assertNull(calibrator.getNedPosition());
1855 assertFalse(calibrator.getNedPosition(null));
1856 assertNull(calibrator.getMeasurements());
1857 assertFalse(calibrator.isCommonAxisUsed());
1858 assertSame(calibrator.getListener(), this);
1859 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1860 assertFalse(calibrator.isReady());
1861 assertFalse(calibrator.isRunning());
1862 assertNull(calibrator.getEstimatedBiases());
1863 assertFalse(calibrator.getEstimatedBiases(null));
1864 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1865 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1866 assertNull(calibrator.getEstimatedBiasFx());
1867 assertNull(calibrator.getEstimatedBiasFy());
1868 assertNull(calibrator.getEstimatedBiasFz());
1869 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1870 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1871 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1872 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1873 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1874 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1875 assertNull(calibrator.getEstimatedMa());
1876 assertNull(calibrator.getEstimatedSx());
1877 assertNull(calibrator.getEstimatedSy());
1878 assertNull(calibrator.getEstimatedSz());
1879 assertNull(calibrator.getEstimatedMxy());
1880 assertNull(calibrator.getEstimatedMxz());
1881 assertNull(calibrator.getEstimatedMyx());
1882 assertNull(calibrator.getEstimatedMyz());
1883 assertNull(calibrator.getEstimatedMzx());
1884 assertNull(calibrator.getEstimatedMzy());
1885 assertNull(calibrator.getEstimatedCovariance());
1886 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1887 assertNull(calibrator.getGroundTruthGravityNorm());
1888 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1889 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1890 }
1891
1892 @Test
1893 public void testConstructor19() throws WrongSizeException {
1894 final Collection<StandardDeviationBodyKinematics> measurements =
1895 Collections.emptyList();
1896
1897 final Matrix ba = generateBa();
1898 final double biasX = ba.getElementAtIndex(0);
1899 final double biasY = ba.getElementAtIndex(1);
1900 final double biasZ = ba.getElementAtIndex(2);
1901
1902 final Acceleration bx = new Acceleration(biasX,
1903 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1904 final Acceleration by = new Acceleration(biasY,
1905 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1906 final Acceleration bz = new Acceleration(biasZ,
1907 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1908
1909 final KnownPositionAccelerometerCalibrator calibrator =
1910 new KnownPositionAccelerometerCalibrator(measurements,
1911 bx, by, bz);
1912
1913
1914 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1915 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1916 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1917 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1918 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1919 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1920 final Acceleration bx2 = new Acceleration(0.0,
1921 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1922 calibrator.getInitialBiasXAsAcceleration(bx2);
1923 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1924 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1925 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1926 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1927 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1928 final Acceleration by2 = new Acceleration(0.0,
1929 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1930 calibrator.getInitialBiasYAsAcceleration(by2);
1931 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1932 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1933 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1934 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1935 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1936 final Acceleration bz2 = new Acceleration(0.0,
1937 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1938 calibrator.getInitialBiasZAsAcceleration(bz2);
1939 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1940 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1941 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1942 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1943 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1944 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1945 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1946 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1947 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1948 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1949 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1950 final double[] bias1 = calibrator.getInitialBias();
1951 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1952 final double[] bias2 = new double[3];
1953 calibrator.getInitialBias(bias2);
1954 assertArrayEquals(bias1, bias2, 0.0);
1955 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1956 assertEquals(b1, ba);
1957 final Matrix b2 = new Matrix(3, 1);
1958 calibrator.getInitialBiasAsMatrix(b2);
1959 assertEquals(b1, b2);
1960 final Matrix ma1 = calibrator.getInitialMa();
1961 assertEquals(ma1, new Matrix(3, 3));
1962 final Matrix ma2 = new Matrix(3, 3);
1963 calibrator.getInitialMa(ma2);
1964 assertEquals(ma1, ma2);
1965 assertNull(calibrator.getEcefPosition());
1966 assertNull(calibrator.getNedPosition());
1967 assertFalse(calibrator.getNedPosition(null));
1968 assertSame(calibrator.getMeasurements(), measurements);
1969 assertFalse(calibrator.isCommonAxisUsed());
1970 assertNull(calibrator.getListener());
1971 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1972 assertFalse(calibrator.isReady());
1973 assertFalse(calibrator.isRunning());
1974 assertNull(calibrator.getEstimatedBiases());
1975 assertFalse(calibrator.getEstimatedBiases(null));
1976 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1977 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1978 assertNull(calibrator.getEstimatedBiasFx());
1979 assertNull(calibrator.getEstimatedBiasFy());
1980 assertNull(calibrator.getEstimatedBiasFz());
1981 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1982 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1983 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1984 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1985 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1986 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1987 assertNull(calibrator.getEstimatedMa());
1988 assertNull(calibrator.getEstimatedSx());
1989 assertNull(calibrator.getEstimatedSy());
1990 assertNull(calibrator.getEstimatedSz());
1991 assertNull(calibrator.getEstimatedMxy());
1992 assertNull(calibrator.getEstimatedMxz());
1993 assertNull(calibrator.getEstimatedMyx());
1994 assertNull(calibrator.getEstimatedMyz());
1995 assertNull(calibrator.getEstimatedMzx());
1996 assertNull(calibrator.getEstimatedMzy());
1997 assertNull(calibrator.getEstimatedCovariance());
1998 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1999 assertNull(calibrator.getGroundTruthGravityNorm());
2000 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2001 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2002 }
2003
2004 @Test
2005 public void testConstructor20() throws WrongSizeException {
2006 final Collection<StandardDeviationBodyKinematics> measurements =
2007 Collections.emptyList();
2008
2009 final Matrix ba = generateBa();
2010 final double biasX = ba.getElementAtIndex(0);
2011 final double biasY = ba.getElementAtIndex(1);
2012 final double biasZ = ba.getElementAtIndex(2);
2013
2014 final Acceleration bx = new Acceleration(biasX,
2015 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2016 final Acceleration by = new Acceleration(biasY,
2017 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2018 final Acceleration bz = new Acceleration(biasZ,
2019 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2020
2021 final KnownPositionAccelerometerCalibrator calibrator =
2022 new KnownPositionAccelerometerCalibrator(measurements,
2023 bx, by, bz, this);
2024
2025
2026 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2027 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2028 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2029 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2030 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2031 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2032 final Acceleration bx2 = new Acceleration(0.0,
2033 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2034 calibrator.getInitialBiasXAsAcceleration(bx2);
2035 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2036 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2037 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2038 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2039 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2040 final Acceleration by2 = new Acceleration(0.0,
2041 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2042 calibrator.getInitialBiasYAsAcceleration(by2);
2043 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2044 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2045 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2046 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2047 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2048 final Acceleration bz2 = new Acceleration(0.0,
2049 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2050 calibrator.getInitialBiasZAsAcceleration(bz2);
2051 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2052 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2053 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2054 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2055 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2056 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2057 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2058 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2059 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2060 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2061 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2062 final double[] bias1 = calibrator.getInitialBias();
2063 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2064 final double[] bias2 = new double[3];
2065 calibrator.getInitialBias(bias2);
2066 assertArrayEquals(bias1, bias2, 0.0);
2067 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2068 assertEquals(b1, ba);
2069 final Matrix b2 = new Matrix(3, 1);
2070 calibrator.getInitialBiasAsMatrix(b2);
2071 assertEquals(b1, b2);
2072 final Matrix ma1 = calibrator.getInitialMa();
2073 assertEquals(ma1, new Matrix(3, 3));
2074 final Matrix ma2 = new Matrix(3, 3);
2075 calibrator.getInitialMa(ma2);
2076 assertEquals(ma1, ma2);
2077 assertNull(calibrator.getEcefPosition());
2078 assertNull(calibrator.getNedPosition());
2079 assertFalse(calibrator.getNedPosition(null));
2080 assertSame(calibrator.getMeasurements(), measurements);
2081 assertFalse(calibrator.isCommonAxisUsed());
2082 assertSame(calibrator.getListener(), this);
2083 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2084 assertFalse(calibrator.isReady());
2085 assertFalse(calibrator.isRunning());
2086 assertNull(calibrator.getEstimatedBiases());
2087 assertFalse(calibrator.getEstimatedBiases(null));
2088 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2089 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2090 assertNull(calibrator.getEstimatedBiasFx());
2091 assertNull(calibrator.getEstimatedBiasFy());
2092 assertNull(calibrator.getEstimatedBiasFz());
2093 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2094 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2095 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2096 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2097 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2098 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2099 assertNull(calibrator.getEstimatedMa());
2100 assertNull(calibrator.getEstimatedSx());
2101 assertNull(calibrator.getEstimatedSy());
2102 assertNull(calibrator.getEstimatedSz());
2103 assertNull(calibrator.getEstimatedMxy());
2104 assertNull(calibrator.getEstimatedMxz());
2105 assertNull(calibrator.getEstimatedMyx());
2106 assertNull(calibrator.getEstimatedMyz());
2107 assertNull(calibrator.getEstimatedMzx());
2108 assertNull(calibrator.getEstimatedMzy());
2109 assertNull(calibrator.getEstimatedCovariance());
2110 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2111 assertNull(calibrator.getGroundTruthGravityNorm());
2112 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2113 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2114 }
2115
2116 @Test
2117 public void testConstructor21() throws WrongSizeException {
2118 final Matrix ba = generateBa();
2119 final double biasX = ba.getElementAtIndex(0);
2120 final double biasY = ba.getElementAtIndex(1);
2121 final double biasZ = ba.getElementAtIndex(2);
2122
2123 final Acceleration bx = new Acceleration(biasX,
2124 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2125 final Acceleration by = new Acceleration(biasY,
2126 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2127 final Acceleration bz = new Acceleration(biasZ,
2128 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2129
2130 final KnownPositionAccelerometerCalibrator calibrator =
2131 new KnownPositionAccelerometerCalibrator(true,
2132 bx, by, bz);
2133
2134
2135 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2136 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2137 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2138 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2139 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2140 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2141 final Acceleration bx2 = new Acceleration(0.0,
2142 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2143 calibrator.getInitialBiasXAsAcceleration(bx2);
2144 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2145 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2146 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2147 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2148 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2149 final Acceleration by2 = new Acceleration(0.0,
2150 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2151 calibrator.getInitialBiasYAsAcceleration(by2);
2152 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2153 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2154 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2155 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2156 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2157 final Acceleration bz2 = new Acceleration(0.0,
2158 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2159 calibrator.getInitialBiasZAsAcceleration(bz2);
2160 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2161 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2162 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2163 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2164 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2165 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2166 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2167 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2168 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2169 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2170 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2171 final double[] bias1 = calibrator.getInitialBias();
2172 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2173 final double[] bias2 = new double[3];
2174 calibrator.getInitialBias(bias2);
2175 assertArrayEquals(bias1, bias2, 0.0);
2176 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2177 assertEquals(b1, ba);
2178 final Matrix b2 = new Matrix(3, 1);
2179 calibrator.getInitialBiasAsMatrix(b2);
2180 assertEquals(b1, b2);
2181 final Matrix ma1 = calibrator.getInitialMa();
2182 assertEquals(ma1, new Matrix(3, 3));
2183 final Matrix ma2 = new Matrix(3, 3);
2184 calibrator.getInitialMa(ma2);
2185 assertEquals(ma1, ma2);
2186 assertNull(calibrator.getEcefPosition());
2187 assertNull(calibrator.getNedPosition());
2188 assertFalse(calibrator.getNedPosition(null));
2189 assertNull(calibrator.getMeasurements());
2190 assertTrue(calibrator.isCommonAxisUsed());
2191 assertNull(calibrator.getListener());
2192 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2193 assertFalse(calibrator.isReady());
2194 assertFalse(calibrator.isRunning());
2195 assertNull(calibrator.getEstimatedBiases());
2196 assertFalse(calibrator.getEstimatedBiases(null));
2197 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2198 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2199 assertNull(calibrator.getEstimatedBiasFx());
2200 assertNull(calibrator.getEstimatedBiasFy());
2201 assertNull(calibrator.getEstimatedBiasFz());
2202 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2203 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2204 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2205 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2206 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2207 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2208 assertNull(calibrator.getEstimatedMa());
2209 assertNull(calibrator.getEstimatedSx());
2210 assertNull(calibrator.getEstimatedSy());
2211 assertNull(calibrator.getEstimatedSz());
2212 assertNull(calibrator.getEstimatedMxy());
2213 assertNull(calibrator.getEstimatedMxz());
2214 assertNull(calibrator.getEstimatedMyx());
2215 assertNull(calibrator.getEstimatedMyz());
2216 assertNull(calibrator.getEstimatedMzx());
2217 assertNull(calibrator.getEstimatedMzy());
2218 assertNull(calibrator.getEstimatedCovariance());
2219 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2220 assertNull(calibrator.getGroundTruthGravityNorm());
2221 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2222 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2223 }
2224
2225 @Test
2226 public void testConstructor22() throws WrongSizeException {
2227 final Matrix ba = generateBa();
2228 final double biasX = ba.getElementAtIndex(0);
2229 final double biasY = ba.getElementAtIndex(1);
2230 final double biasZ = ba.getElementAtIndex(2);
2231
2232 final Acceleration bx = new Acceleration(biasX,
2233 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2234 final Acceleration by = new Acceleration(biasY,
2235 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2236 final Acceleration bz = new Acceleration(biasZ,
2237 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2238
2239 final KnownPositionAccelerometerCalibrator calibrator =
2240 new KnownPositionAccelerometerCalibrator(true,
2241 bx, by, bz, this);
2242
2243
2244 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2245 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2246 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2247 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2248 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2249 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2250 final Acceleration bx2 = new Acceleration(0.0,
2251 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2252 calibrator.getInitialBiasXAsAcceleration(bx2);
2253 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2254 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2255 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2256 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2257 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2258 final Acceleration by2 = new Acceleration(0.0,
2259 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2260 calibrator.getInitialBiasYAsAcceleration(by2);
2261 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2262 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2263 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2264 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2265 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2266 final Acceleration bz2 = new Acceleration(0.0,
2267 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2268 calibrator.getInitialBiasZAsAcceleration(bz2);
2269 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2270 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2271 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2272 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2273 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2274 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2275 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2276 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2277 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2278 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2279 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2280 final double[] bias1 = calibrator.getInitialBias();
2281 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2282 final double[] bias2 = new double[3];
2283 calibrator.getInitialBias(bias2);
2284 assertArrayEquals(bias1, bias2, 0.0);
2285 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2286 assertEquals(b1, ba);
2287 final Matrix b2 = new Matrix(3, 1);
2288 calibrator.getInitialBiasAsMatrix(b2);
2289 assertEquals(b1, b2);
2290 final Matrix ma1 = calibrator.getInitialMa();
2291 assertEquals(ma1, new Matrix(3, 3));
2292 final Matrix ma2 = new Matrix(3, 3);
2293 calibrator.getInitialMa(ma2);
2294 assertEquals(ma1, ma2);
2295 assertNull(calibrator.getEcefPosition());
2296 assertNull(calibrator.getNedPosition());
2297 assertFalse(calibrator.getNedPosition(null));
2298 assertNull(calibrator.getMeasurements());
2299 assertTrue(calibrator.isCommonAxisUsed());
2300 assertSame(calibrator.getListener(), this);
2301 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2302 assertFalse(calibrator.isReady());
2303 assertFalse(calibrator.isRunning());
2304 assertNull(calibrator.getEstimatedBiases());
2305 assertFalse(calibrator.getEstimatedBiases(null));
2306 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2307 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2308 assertNull(calibrator.getEstimatedBiasFx());
2309 assertNull(calibrator.getEstimatedBiasFy());
2310 assertNull(calibrator.getEstimatedBiasFz());
2311 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2312 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2313 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2314 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2315 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2316 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2317 assertNull(calibrator.getEstimatedMa());
2318 assertNull(calibrator.getEstimatedSx());
2319 assertNull(calibrator.getEstimatedSy());
2320 assertNull(calibrator.getEstimatedSz());
2321 assertNull(calibrator.getEstimatedMxy());
2322 assertNull(calibrator.getEstimatedMxz());
2323 assertNull(calibrator.getEstimatedMyx());
2324 assertNull(calibrator.getEstimatedMyz());
2325 assertNull(calibrator.getEstimatedMzx());
2326 assertNull(calibrator.getEstimatedMzy());
2327 assertNull(calibrator.getEstimatedCovariance());
2328 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2329 assertNull(calibrator.getGroundTruthGravityNorm());
2330 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2331 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2332 }
2333
2334 @Test
2335 public void testConstructor23() throws WrongSizeException {
2336 final Collection<StandardDeviationBodyKinematics> measurements =
2337 Collections.emptyList();
2338
2339 final Matrix ba = generateBa();
2340 final double biasX = ba.getElementAtIndex(0);
2341 final double biasY = ba.getElementAtIndex(1);
2342 final double biasZ = ba.getElementAtIndex(2);
2343
2344 final Acceleration bx = new Acceleration(biasX,
2345 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2346 final Acceleration by = new Acceleration(biasY,
2347 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2348 final Acceleration bz = new Acceleration(biasZ,
2349 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2350
2351 final KnownPositionAccelerometerCalibrator calibrator =
2352 new KnownPositionAccelerometerCalibrator(measurements,
2353 true, bx, by, bz);
2354
2355
2356 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2357 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2358 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2359 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2360 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2361 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2362 final Acceleration bx2 = new Acceleration(0.0,
2363 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2364 calibrator.getInitialBiasXAsAcceleration(bx2);
2365 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2366 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2367 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2368 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2369 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2370 final Acceleration by2 = new Acceleration(0.0,
2371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2372 calibrator.getInitialBiasYAsAcceleration(by2);
2373 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2374 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2375 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2376 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2377 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2378 final Acceleration bz2 = new Acceleration(0.0,
2379 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2380 calibrator.getInitialBiasZAsAcceleration(bz2);
2381 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2382 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2383 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2384 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2385 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2386 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2387 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2388 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2389 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2390 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2391 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2392 final double[] bias1 = calibrator.getInitialBias();
2393 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2394 final double[] bias2 = new double[3];
2395 calibrator.getInitialBias(bias2);
2396 assertArrayEquals(bias1, bias2, 0.0);
2397 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2398 assertEquals(b1, ba);
2399 final Matrix b2 = new Matrix(3, 1);
2400 calibrator.getInitialBiasAsMatrix(b2);
2401 assertEquals(b1, b2);
2402 final Matrix ma1 = calibrator.getInitialMa();
2403 assertEquals(ma1, new Matrix(3, 3));
2404 final Matrix ma2 = new Matrix(3, 3);
2405 calibrator.getInitialMa(ma2);
2406 assertEquals(ma1, ma2);
2407 assertNull(calibrator.getEcefPosition());
2408 assertNull(calibrator.getNedPosition());
2409 assertFalse(calibrator.getNedPosition(null));
2410 assertSame(calibrator.getMeasurements(), measurements);
2411 assertTrue(calibrator.isCommonAxisUsed());
2412 assertNull(calibrator.getListener());
2413 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2414 assertFalse(calibrator.isReady());
2415 assertFalse(calibrator.isRunning());
2416 assertNull(calibrator.getEstimatedBiases());
2417 assertFalse(calibrator.getEstimatedBiases(null));
2418 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2419 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2420 assertNull(calibrator.getEstimatedBiasFx());
2421 assertNull(calibrator.getEstimatedBiasFy());
2422 assertNull(calibrator.getEstimatedBiasFz());
2423 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2424 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2425 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2426 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2427 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2428 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2429 assertNull(calibrator.getEstimatedMa());
2430 assertNull(calibrator.getEstimatedSx());
2431 assertNull(calibrator.getEstimatedSy());
2432 assertNull(calibrator.getEstimatedSz());
2433 assertNull(calibrator.getEstimatedMxy());
2434 assertNull(calibrator.getEstimatedMxz());
2435 assertNull(calibrator.getEstimatedMyx());
2436 assertNull(calibrator.getEstimatedMyz());
2437 assertNull(calibrator.getEstimatedMzx());
2438 assertNull(calibrator.getEstimatedMzy());
2439 assertNull(calibrator.getEstimatedCovariance());
2440 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2441 assertNull(calibrator.getGroundTruthGravityNorm());
2442 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2443 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2444 }
2445
2446 @Test
2447 public void testConstructor24() throws WrongSizeException {
2448 final Collection<StandardDeviationBodyKinematics> measurements =
2449 Collections.emptyList();
2450
2451 final Matrix ba = generateBa();
2452 final double biasX = ba.getElementAtIndex(0);
2453 final double biasY = ba.getElementAtIndex(1);
2454 final double biasZ = ba.getElementAtIndex(2);
2455
2456 final Acceleration bx = new Acceleration(biasX,
2457 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2458 final Acceleration by = new Acceleration(biasY,
2459 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2460 final Acceleration bz = new Acceleration(biasZ,
2461 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2462
2463 final KnownPositionAccelerometerCalibrator calibrator =
2464 new KnownPositionAccelerometerCalibrator(measurements,
2465 true, bx, by, bz, this);
2466
2467
2468 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2469 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2470 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2471 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2472 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2473 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2474 final Acceleration bx2 = new Acceleration(0.0,
2475 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2476 calibrator.getInitialBiasXAsAcceleration(bx2);
2477 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2478 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2479 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2480 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2481 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2482 final Acceleration by2 = new Acceleration(0.0,
2483 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2484 calibrator.getInitialBiasYAsAcceleration(by2);
2485 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2486 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2487 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2488 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2489 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2490 final Acceleration bz2 = new Acceleration(0.0,
2491 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2492 calibrator.getInitialBiasZAsAcceleration(bz2);
2493 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2494 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2495 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2496 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2497 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2498 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2499 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2500 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2501 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2502 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2503 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2504 final double[] bias1 = calibrator.getInitialBias();
2505 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2506 final double[] bias2 = new double[3];
2507 calibrator.getInitialBias(bias2);
2508 assertArrayEquals(bias1, bias2, 0.0);
2509 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2510 assertEquals(b1, ba);
2511 final Matrix b2 = new Matrix(3, 1);
2512 calibrator.getInitialBiasAsMatrix(b2);
2513 assertEquals(b1, b2);
2514 final Matrix ma1 = calibrator.getInitialMa();
2515 assertEquals(ma1, new Matrix(3, 3));
2516 final Matrix ma2 = new Matrix(3, 3);
2517 calibrator.getInitialMa(ma2);
2518 assertEquals(ma1, ma2);
2519 assertNull(calibrator.getEcefPosition());
2520 assertNull(calibrator.getNedPosition());
2521 assertFalse(calibrator.getNedPosition(null));
2522 assertSame(calibrator.getMeasurements(), measurements);
2523 assertTrue(calibrator.isCommonAxisUsed());
2524 assertSame(calibrator.getListener(), this);
2525 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2526 assertFalse(calibrator.isReady());
2527 assertFalse(calibrator.isRunning());
2528 assertNull(calibrator.getEstimatedBiases());
2529 assertFalse(calibrator.getEstimatedBiases(null));
2530 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2531 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2532 assertNull(calibrator.getEstimatedBiasFx());
2533 assertNull(calibrator.getEstimatedBiasFy());
2534 assertNull(calibrator.getEstimatedBiasFz());
2535 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2536 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2537 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2538 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2539 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2540 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2541 assertNull(calibrator.getEstimatedMa());
2542 assertNull(calibrator.getEstimatedSx());
2543 assertNull(calibrator.getEstimatedSy());
2544 assertNull(calibrator.getEstimatedSz());
2545 assertNull(calibrator.getEstimatedMxy());
2546 assertNull(calibrator.getEstimatedMxz());
2547 assertNull(calibrator.getEstimatedMyx());
2548 assertNull(calibrator.getEstimatedMyz());
2549 assertNull(calibrator.getEstimatedMzx());
2550 assertNull(calibrator.getEstimatedMzy());
2551 assertNull(calibrator.getEstimatedCovariance());
2552 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2553 assertNull(calibrator.getGroundTruthGravityNorm());
2554 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2555 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2556 }
2557
2558 @Test
2559 public void testConstructor25() throws WrongSizeException {
2560 final Matrix ba = generateBa();
2561 final double biasX = ba.getElementAtIndex(0);
2562 final double biasY = ba.getElementAtIndex(1);
2563 final double biasZ = ba.getElementAtIndex(2);
2564
2565 final Matrix ma = generateMaCommonAxis();
2566 final double sx = ma.getElementAt(0, 0);
2567 final double sy = ma.getElementAt(1, 1);
2568 final double sz = ma.getElementAt(2, 2);
2569
2570 final KnownPositionAccelerometerCalibrator calibrator =
2571 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
2572 sx, sy, sz);
2573
2574
2575 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2576 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2577 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2578 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2579 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2580 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2581 final Acceleration bx2 = new Acceleration(0.0,
2582 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2583 calibrator.getInitialBiasXAsAcceleration(bx2);
2584 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2585 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2586 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2587 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2588 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2589 final Acceleration by2 = new Acceleration(0.0,
2590 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2591 calibrator.getInitialBiasYAsAcceleration(by2);
2592 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2593 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2594 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2595 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2596 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2597 final Acceleration bz2 = new Acceleration(0.0,
2598 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2599 calibrator.getInitialBiasZAsAcceleration(bz2);
2600 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2601 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2602 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2603 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2604 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2605 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2606 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2607 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2608 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2609 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2610 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2611 final double[] bias1 = calibrator.getInitialBias();
2612 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2613 final double[] bias2 = new double[3];
2614 calibrator.getInitialBias(bias2);
2615 assertArrayEquals(bias1, bias2, 0.0);
2616 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2617 assertEquals(b1, ba);
2618 final Matrix b2 = new Matrix(3, 1);
2619 calibrator.getInitialBiasAsMatrix(b2);
2620 assertEquals(b1, b2);
2621 final Matrix ma1 = calibrator.getInitialMa();
2622 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2623 final Matrix ma2 = new Matrix(3, 3);
2624 calibrator.getInitialMa(ma2);
2625 assertEquals(ma1, ma2);
2626 assertNull(calibrator.getEcefPosition());
2627 assertNull(calibrator.getNedPosition());
2628 assertFalse(calibrator.getNedPosition(null));
2629 assertNull(calibrator.getMeasurements());
2630 assertFalse(calibrator.isCommonAxisUsed());
2631 assertNull(calibrator.getListener());
2632 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2633 assertFalse(calibrator.isReady());
2634 assertFalse(calibrator.isRunning());
2635 assertNull(calibrator.getEstimatedBiases());
2636 assertFalse(calibrator.getEstimatedBiases(null));
2637 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2638 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2639 assertNull(calibrator.getEstimatedBiasFx());
2640 assertNull(calibrator.getEstimatedBiasFy());
2641 assertNull(calibrator.getEstimatedBiasFz());
2642 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2643 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2644 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2645 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2646 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2647 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2648 assertNull(calibrator.getEstimatedMa());
2649 assertNull(calibrator.getEstimatedSx());
2650 assertNull(calibrator.getEstimatedSy());
2651 assertNull(calibrator.getEstimatedSz());
2652 assertNull(calibrator.getEstimatedMxy());
2653 assertNull(calibrator.getEstimatedMxz());
2654 assertNull(calibrator.getEstimatedMyx());
2655 assertNull(calibrator.getEstimatedMyz());
2656 assertNull(calibrator.getEstimatedMzx());
2657 assertNull(calibrator.getEstimatedMzy());
2658 assertNull(calibrator.getEstimatedCovariance());
2659 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2660 assertNull(calibrator.getGroundTruthGravityNorm());
2661 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2662 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2663 }
2664
2665 @Test
2666 public void testConstructor26() throws WrongSizeException {
2667 final Collection<StandardDeviationBodyKinematics> measurements =
2668 Collections.emptyList();
2669
2670 final Matrix ba = generateBa();
2671 final double biasX = ba.getElementAtIndex(0);
2672 final double biasY = ba.getElementAtIndex(1);
2673 final double biasZ = ba.getElementAtIndex(2);
2674
2675 final Matrix ma = generateMaCommonAxis();
2676 final double sx = ma.getElementAt(0, 0);
2677 final double sy = ma.getElementAt(1, 1);
2678 final double sz = ma.getElementAt(2, 2);
2679
2680 final KnownPositionAccelerometerCalibrator calibrator =
2681 new KnownPositionAccelerometerCalibrator(measurements,
2682 biasX, biasY, biasZ, sx, sy, sz);
2683
2684
2685 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2686 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2687 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2688 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2689 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2690 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2691 final Acceleration bx2 = new Acceleration(0.0,
2692 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2693 calibrator.getInitialBiasXAsAcceleration(bx2);
2694 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2695 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2696 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2697 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2698 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2699 final Acceleration by2 = new Acceleration(0.0,
2700 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2701 calibrator.getInitialBiasYAsAcceleration(by2);
2702 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2703 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2704 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2705 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2706 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2707 final Acceleration bz2 = new Acceleration(0.0,
2708 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2709 calibrator.getInitialBiasZAsAcceleration(bz2);
2710 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2711 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2712 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2713 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2714 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2715 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2716 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2717 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2718 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2719 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2720 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2721 final double[] bias1 = calibrator.getInitialBias();
2722 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2723 final double[] bias2 = new double[3];
2724 calibrator.getInitialBias(bias2);
2725 assertArrayEquals(bias1, bias2, 0.0);
2726 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2727 assertEquals(b1, ba);
2728 final Matrix b2 = new Matrix(3, 1);
2729 calibrator.getInitialBiasAsMatrix(b2);
2730 assertEquals(b1, b2);
2731 final Matrix ma1 = calibrator.getInitialMa();
2732 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2733 final Matrix ma2 = new Matrix(3, 3);
2734 calibrator.getInitialMa(ma2);
2735 assertEquals(ma1, ma2);
2736 assertNull(calibrator.getEcefPosition());
2737 assertNull(calibrator.getNedPosition());
2738 assertFalse(calibrator.getNedPosition(null));
2739 assertSame(calibrator.getMeasurements(), measurements);
2740 assertFalse(calibrator.isCommonAxisUsed());
2741 assertNull(calibrator.getListener());
2742 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2743 assertFalse(calibrator.isReady());
2744 assertFalse(calibrator.isRunning());
2745 assertNull(calibrator.getEstimatedBiases());
2746 assertFalse(calibrator.getEstimatedBiases(null));
2747 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2748 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2749 assertNull(calibrator.getEstimatedBiasFx());
2750 assertNull(calibrator.getEstimatedBiasFy());
2751 assertNull(calibrator.getEstimatedBiasFz());
2752 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2753 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2754 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2755 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2756 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2757 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2758 assertNull(calibrator.getEstimatedMa());
2759 assertNull(calibrator.getEstimatedSx());
2760 assertNull(calibrator.getEstimatedSy());
2761 assertNull(calibrator.getEstimatedSz());
2762 assertNull(calibrator.getEstimatedMxy());
2763 assertNull(calibrator.getEstimatedMxz());
2764 assertNull(calibrator.getEstimatedMyx());
2765 assertNull(calibrator.getEstimatedMyz());
2766 assertNull(calibrator.getEstimatedMzx());
2767 assertNull(calibrator.getEstimatedMzy());
2768 assertNull(calibrator.getEstimatedCovariance());
2769 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2770 assertNull(calibrator.getGroundTruthGravityNorm());
2771 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2772 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2773 }
2774
2775 @Test
2776 public void testConstructor27() throws WrongSizeException {
2777 final Collection<StandardDeviationBodyKinematics> measurements =
2778 Collections.emptyList();
2779
2780 final Matrix ba = generateBa();
2781 final double biasX = ba.getElementAtIndex(0);
2782 final double biasY = ba.getElementAtIndex(1);
2783 final double biasZ = ba.getElementAtIndex(2);
2784
2785 final Matrix ma = generateMaCommonAxis();
2786 final double sx = ma.getElementAt(0, 0);
2787 final double sy = ma.getElementAt(1, 1);
2788 final double sz = ma.getElementAt(2, 2);
2789
2790 final KnownPositionAccelerometerCalibrator calibrator =
2791 new KnownPositionAccelerometerCalibrator(measurements,
2792 biasX, biasY, biasZ, sx, sy, sz, this);
2793
2794
2795 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2796 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2797 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2798 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2799 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2800 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2801 final Acceleration bx2 = new Acceleration(0.0,
2802 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2803 calibrator.getInitialBiasXAsAcceleration(bx2);
2804 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2805 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2806 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2807 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2808 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2809 final Acceleration by2 = new Acceleration(0.0,
2810 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2811 calibrator.getInitialBiasYAsAcceleration(by2);
2812 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2813 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2814 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2815 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2816 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2817 final Acceleration bz2 = new Acceleration(0.0,
2818 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2819 calibrator.getInitialBiasZAsAcceleration(bz2);
2820 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2821 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2822 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2823 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2824 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2825 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2826 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2827 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2828 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2829 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2830 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2831 final double[] bias1 = calibrator.getInitialBias();
2832 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2833 final double[] bias2 = new double[3];
2834 calibrator.getInitialBias(bias2);
2835 assertArrayEquals(bias1, bias2, 0.0);
2836 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2837 assertEquals(b1, ba);
2838 final Matrix b2 = new Matrix(3, 1);
2839 calibrator.getInitialBiasAsMatrix(b2);
2840 assertEquals(b1, b2);
2841 final Matrix ma1 = calibrator.getInitialMa();
2842 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2843 final Matrix ma2 = new Matrix(3, 3);
2844 calibrator.getInitialMa(ma2);
2845 assertEquals(ma1, ma2);
2846 assertNull(calibrator.getEcefPosition());
2847 assertNull(calibrator.getNedPosition());
2848 assertFalse(calibrator.getNedPosition(null));
2849 assertSame(calibrator.getMeasurements(), measurements);
2850 assertFalse(calibrator.isCommonAxisUsed());
2851 assertSame(calibrator.getListener(), this);
2852 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2853 assertFalse(calibrator.isReady());
2854 assertFalse(calibrator.isRunning());
2855 assertNull(calibrator.getEstimatedBiases());
2856 assertFalse(calibrator.getEstimatedBiases(null));
2857 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2858 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2859 assertNull(calibrator.getEstimatedBiasFx());
2860 assertNull(calibrator.getEstimatedBiasFy());
2861 assertNull(calibrator.getEstimatedBiasFz());
2862 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2863 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2864 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2865 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2866 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2867 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2868 assertNull(calibrator.getEstimatedMa());
2869 assertNull(calibrator.getEstimatedSx());
2870 assertNull(calibrator.getEstimatedSy());
2871 assertNull(calibrator.getEstimatedSz());
2872 assertNull(calibrator.getEstimatedMxy());
2873 assertNull(calibrator.getEstimatedMxz());
2874 assertNull(calibrator.getEstimatedMyx());
2875 assertNull(calibrator.getEstimatedMyz());
2876 assertNull(calibrator.getEstimatedMzx());
2877 assertNull(calibrator.getEstimatedMzy());
2878 assertNull(calibrator.getEstimatedCovariance());
2879 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2880 assertNull(calibrator.getGroundTruthGravityNorm());
2881 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2882 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2883 }
2884
2885 @Test
2886 public void testConstructor28() throws WrongSizeException {
2887 final Matrix ba = generateBa();
2888 final double biasX = ba.getElementAtIndex(0);
2889 final double biasY = ba.getElementAtIndex(1);
2890 final double biasZ = ba.getElementAtIndex(2);
2891
2892 final Matrix ma = generateMaCommonAxis();
2893 final double sx = ma.getElementAt(0, 0);
2894 final double sy = ma.getElementAt(1, 1);
2895 final double sz = ma.getElementAt(2, 2);
2896
2897 final KnownPositionAccelerometerCalibrator calibrator =
2898 new KnownPositionAccelerometerCalibrator(true,
2899 biasX, biasY, biasZ, sx, sy, sz);
2900
2901
2902 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2903 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2904 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2905 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2906 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2907 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2908 final Acceleration bx2 = new Acceleration(0.0,
2909 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2910 calibrator.getInitialBiasXAsAcceleration(bx2);
2911 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2912 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2913 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2914 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2915 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2916 final Acceleration by2 = new Acceleration(0.0,
2917 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2918 calibrator.getInitialBiasYAsAcceleration(by2);
2919 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2920 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2921 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2922 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2923 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2924 final Acceleration bz2 = new Acceleration(0.0,
2925 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2926 calibrator.getInitialBiasZAsAcceleration(bz2);
2927 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2928 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2929 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2930 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2931 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2932 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2933 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2934 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2935 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2936 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2937 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2938 final double[] bias1 = calibrator.getInitialBias();
2939 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2940 final double[] bias2 = new double[3];
2941 calibrator.getInitialBias(bias2);
2942 assertArrayEquals(bias1, bias2, 0.0);
2943 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2944 assertEquals(b1, ba);
2945 final Matrix b2 = new Matrix(3, 1);
2946 calibrator.getInitialBiasAsMatrix(b2);
2947 assertEquals(b1, b2);
2948 final Matrix ma1 = calibrator.getInitialMa();
2949 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2950 final Matrix ma2 = new Matrix(3, 3);
2951 calibrator.getInitialMa(ma2);
2952 assertEquals(ma1, ma2);
2953 assertNull(calibrator.getEcefPosition());
2954 assertNull(calibrator.getNedPosition());
2955 assertFalse(calibrator.getNedPosition(null));
2956 assertNull(calibrator.getMeasurements());
2957 assertTrue(calibrator.isCommonAxisUsed());
2958 assertNull(calibrator.getListener());
2959 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2960 assertFalse(calibrator.isReady());
2961 assertFalse(calibrator.isRunning());
2962 assertNull(calibrator.getEstimatedBiases());
2963 assertFalse(calibrator.getEstimatedBiases(null));
2964 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2965 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2966 assertNull(calibrator.getEstimatedBiasFx());
2967 assertNull(calibrator.getEstimatedBiasFy());
2968 assertNull(calibrator.getEstimatedBiasFz());
2969 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2970 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2971 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2972 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2973 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2974 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2975 assertNull(calibrator.getEstimatedMa());
2976 assertNull(calibrator.getEstimatedSx());
2977 assertNull(calibrator.getEstimatedSy());
2978 assertNull(calibrator.getEstimatedSz());
2979 assertNull(calibrator.getEstimatedMxy());
2980 assertNull(calibrator.getEstimatedMxz());
2981 assertNull(calibrator.getEstimatedMyx());
2982 assertNull(calibrator.getEstimatedMyz());
2983 assertNull(calibrator.getEstimatedMzx());
2984 assertNull(calibrator.getEstimatedMzy());
2985 assertNull(calibrator.getEstimatedCovariance());
2986 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2987 assertNull(calibrator.getGroundTruthGravityNorm());
2988 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2989 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2990 }
2991
2992 @Test
2993 public void testConstructor29() throws WrongSizeException {
2994 final Matrix ba = generateBa();
2995 final double biasX = ba.getElementAtIndex(0);
2996 final double biasY = ba.getElementAtIndex(1);
2997 final double biasZ = ba.getElementAtIndex(2);
2998
2999 final Matrix ma = generateMaCommonAxis();
3000 final double sx = ma.getElementAt(0, 0);
3001 final double sy = ma.getElementAt(1, 1);
3002 final double sz = ma.getElementAt(2, 2);
3003
3004 final KnownPositionAccelerometerCalibrator calibrator =
3005 new KnownPositionAccelerometerCalibrator(true,
3006 biasX, biasY, biasZ, sx, sy, sz, this);
3007
3008
3009 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3010 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3011 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3012 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3013 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3014 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3015 final Acceleration bx2 = new Acceleration(0.0,
3016 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3017 calibrator.getInitialBiasXAsAcceleration(bx2);
3018 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3019 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3020 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3021 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3022 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3023 final Acceleration by2 = new Acceleration(0.0,
3024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3025 calibrator.getInitialBiasYAsAcceleration(by2);
3026 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3027 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3028 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3029 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3030 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3031 final Acceleration bz2 = new Acceleration(0.0,
3032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3033 calibrator.getInitialBiasZAsAcceleration(bz2);
3034 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3035 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3036 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3037 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3038 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3039 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3040 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3041 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3042 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3043 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3044 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3045 final double[] bias1 = calibrator.getInitialBias();
3046 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3047 final double[] bias2 = new double[3];
3048 calibrator.getInitialBias(bias2);
3049 assertArrayEquals(bias1, bias2, 0.0);
3050 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3051 assertEquals(b1, ba);
3052 final Matrix b2 = new Matrix(3, 1);
3053 calibrator.getInitialBiasAsMatrix(b2);
3054 assertEquals(b1, b2);
3055 final Matrix ma1 = calibrator.getInitialMa();
3056 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3057 final Matrix ma2 = new Matrix(3, 3);
3058 calibrator.getInitialMa(ma2);
3059 assertEquals(ma1, ma2);
3060 assertNull(calibrator.getEcefPosition());
3061 assertNull(calibrator.getNedPosition());
3062 assertFalse(calibrator.getNedPosition(null));
3063 assertNull(calibrator.getMeasurements());
3064 assertTrue(calibrator.isCommonAxisUsed());
3065 assertSame(calibrator.getListener(), this);
3066 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3067 assertFalse(calibrator.isReady());
3068 assertFalse(calibrator.isRunning());
3069 assertNull(calibrator.getEstimatedBiases());
3070 assertFalse(calibrator.getEstimatedBiases(null));
3071 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3072 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3073 assertNull(calibrator.getEstimatedBiasFx());
3074 assertNull(calibrator.getEstimatedBiasFy());
3075 assertNull(calibrator.getEstimatedBiasFz());
3076 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3077 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3078 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3079 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3080 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3081 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3082 assertNull(calibrator.getEstimatedMa());
3083 assertNull(calibrator.getEstimatedSx());
3084 assertNull(calibrator.getEstimatedSy());
3085 assertNull(calibrator.getEstimatedSz());
3086 assertNull(calibrator.getEstimatedMxy());
3087 assertNull(calibrator.getEstimatedMxz());
3088 assertNull(calibrator.getEstimatedMyx());
3089 assertNull(calibrator.getEstimatedMyz());
3090 assertNull(calibrator.getEstimatedMzx());
3091 assertNull(calibrator.getEstimatedMzy());
3092 assertNull(calibrator.getEstimatedCovariance());
3093 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3094 assertNull(calibrator.getGroundTruthGravityNorm());
3095 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3096 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3097 }
3098
3099 @Test
3100 public void testConstructor30() throws WrongSizeException {
3101 final Collection<StandardDeviationBodyKinematics> measurements =
3102 Collections.emptyList();
3103 final Matrix ba = generateBa();
3104 final double biasX = ba.getElementAtIndex(0);
3105 final double biasY = ba.getElementAtIndex(1);
3106 final double biasZ = ba.getElementAtIndex(2);
3107
3108 final Matrix ma = generateMaCommonAxis();
3109 final double sx = ma.getElementAt(0, 0);
3110 final double sy = ma.getElementAt(1, 1);
3111 final double sz = ma.getElementAt(2, 2);
3112
3113 final KnownPositionAccelerometerCalibrator calibrator =
3114 new KnownPositionAccelerometerCalibrator(measurements,
3115 true, biasX, biasY, biasZ, sx, sy, sz);
3116
3117
3118 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3119 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3120 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3121 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3122 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3123 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3124 final Acceleration bx2 = new Acceleration(0.0,
3125 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3126 calibrator.getInitialBiasXAsAcceleration(bx2);
3127 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3128 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3129 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3130 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3131 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3132 final Acceleration by2 = new Acceleration(0.0,
3133 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3134 calibrator.getInitialBiasYAsAcceleration(by2);
3135 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3136 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3137 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3138 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3139 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3140 final Acceleration bz2 = new Acceleration(0.0,
3141 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3142 calibrator.getInitialBiasZAsAcceleration(bz2);
3143 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3144 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3145 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3146 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3147 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3148 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3149 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3150 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3151 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3152 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3153 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3154 final double[] bias1 = calibrator.getInitialBias();
3155 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3156 final double[] bias2 = new double[3];
3157 calibrator.getInitialBias(bias2);
3158 assertArrayEquals(bias1, bias2, 0.0);
3159 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3160 assertEquals(b1, ba);
3161 final Matrix b2 = new Matrix(3, 1);
3162 calibrator.getInitialBiasAsMatrix(b2);
3163 assertEquals(b1, b2);
3164 final Matrix ma1 = calibrator.getInitialMa();
3165 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3166 final Matrix ma2 = new Matrix(3, 3);
3167 calibrator.getInitialMa(ma2);
3168 assertEquals(ma1, ma2);
3169 assertNull(calibrator.getEcefPosition());
3170 assertNull(calibrator.getNedPosition());
3171 assertFalse(calibrator.getNedPosition(null));
3172 assertSame(calibrator.getMeasurements(), measurements);
3173 assertTrue(calibrator.isCommonAxisUsed());
3174 assertNull(calibrator.getListener());
3175 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3176 assertFalse(calibrator.isReady());
3177 assertFalse(calibrator.isRunning());
3178 assertNull(calibrator.getEstimatedBiases());
3179 assertFalse(calibrator.getEstimatedBiases(null));
3180 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3181 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3182 assertNull(calibrator.getEstimatedBiasFx());
3183 assertNull(calibrator.getEstimatedBiasFy());
3184 assertNull(calibrator.getEstimatedBiasFz());
3185 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3186 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3187 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3188 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3189 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3190 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3191 assertNull(calibrator.getEstimatedMa());
3192 assertNull(calibrator.getEstimatedSx());
3193 assertNull(calibrator.getEstimatedSy());
3194 assertNull(calibrator.getEstimatedSz());
3195 assertNull(calibrator.getEstimatedMxy());
3196 assertNull(calibrator.getEstimatedMxz());
3197 assertNull(calibrator.getEstimatedMyx());
3198 assertNull(calibrator.getEstimatedMyz());
3199 assertNull(calibrator.getEstimatedMzx());
3200 assertNull(calibrator.getEstimatedMzy());
3201 assertNull(calibrator.getEstimatedCovariance());
3202 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3203 assertNull(calibrator.getGroundTruthGravityNorm());
3204 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3205 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3206 }
3207
3208 @Test
3209 public void testConstructor31() throws WrongSizeException {
3210 final Collection<StandardDeviationBodyKinematics> measurements =
3211 Collections.emptyList();
3212 final Matrix ba = generateBa();
3213 final double biasX = ba.getElementAtIndex(0);
3214 final double biasY = ba.getElementAtIndex(1);
3215 final double biasZ = ba.getElementAtIndex(2);
3216
3217 final Matrix ma = generateMaCommonAxis();
3218 final double sx = ma.getElementAt(0, 0);
3219 final double sy = ma.getElementAt(1, 1);
3220 final double sz = ma.getElementAt(2, 2);
3221
3222 final KnownPositionAccelerometerCalibrator calibrator =
3223 new KnownPositionAccelerometerCalibrator(measurements,
3224 true, biasX, biasY, biasZ, sx, sy, sz,
3225 this);
3226
3227
3228 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3229 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3230 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3231 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3232 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3233 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3234 final Acceleration bx2 = new Acceleration(0.0,
3235 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3236 calibrator.getInitialBiasXAsAcceleration(bx2);
3237 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3238 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3239 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3240 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3241 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3242 final Acceleration by2 = new Acceleration(0.0,
3243 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3244 calibrator.getInitialBiasYAsAcceleration(by2);
3245 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3246 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3247 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3248 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3249 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3250 final Acceleration bz2 = new Acceleration(0.0,
3251 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3252 calibrator.getInitialBiasZAsAcceleration(bz2);
3253 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3254 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3255 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3256 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3257 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3258 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3259 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3260 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3261 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3262 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3263 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3264 final double[] bias1 = calibrator.getInitialBias();
3265 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3266 final double[] bias2 = new double[3];
3267 calibrator.getInitialBias(bias2);
3268 assertArrayEquals(bias1, bias2, 0.0);
3269 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3270 assertEquals(b1, ba);
3271 final Matrix b2 = new Matrix(3, 1);
3272 calibrator.getInitialBiasAsMatrix(b2);
3273 assertEquals(b1, b2);
3274 final Matrix ma1 = calibrator.getInitialMa();
3275 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3276 final Matrix ma2 = new Matrix(3, 3);
3277 calibrator.getInitialMa(ma2);
3278 assertEquals(ma1, ma2);
3279 assertNull(calibrator.getEcefPosition());
3280 assertNull(calibrator.getNedPosition());
3281 assertFalse(calibrator.getNedPosition(null));
3282 assertSame(calibrator.getMeasurements(), measurements);
3283 assertTrue(calibrator.isCommonAxisUsed());
3284 assertSame(calibrator.getListener(), this);
3285 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3286 assertFalse(calibrator.isReady());
3287 assertFalse(calibrator.isRunning());
3288 assertNull(calibrator.getEstimatedBiases());
3289 assertFalse(calibrator.getEstimatedBiases(null));
3290 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3291 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3292 assertNull(calibrator.getEstimatedBiasFx());
3293 assertNull(calibrator.getEstimatedBiasFy());
3294 assertNull(calibrator.getEstimatedBiasFz());
3295 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3296 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3297 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3298 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3299 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3300 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3301 assertNull(calibrator.getEstimatedMa());
3302 assertNull(calibrator.getEstimatedSx());
3303 assertNull(calibrator.getEstimatedSy());
3304 assertNull(calibrator.getEstimatedSz());
3305 assertNull(calibrator.getEstimatedMxy());
3306 assertNull(calibrator.getEstimatedMxz());
3307 assertNull(calibrator.getEstimatedMyx());
3308 assertNull(calibrator.getEstimatedMyz());
3309 assertNull(calibrator.getEstimatedMzx());
3310 assertNull(calibrator.getEstimatedMzy());
3311 assertNull(calibrator.getEstimatedCovariance());
3312 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3313 assertNull(calibrator.getGroundTruthGravityNorm());
3314 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3315 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3316 }
3317
3318 @Test
3319 public void testConstructor32() throws WrongSizeException {
3320 final Matrix ba = generateBa();
3321 final double biasX = ba.getElementAtIndex(0);
3322 final double biasY = ba.getElementAtIndex(1);
3323 final double biasZ = ba.getElementAtIndex(2);
3324
3325 final Matrix ma = generateMaCommonAxis();
3326 final double sx = ma.getElementAt(0, 0);
3327 final double sy = ma.getElementAt(1, 1);
3328 final double sz = ma.getElementAt(2, 2);
3329
3330 final Acceleration bx = new Acceleration(biasX,
3331 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3332 final Acceleration by = new Acceleration(biasY,
3333 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3334 final Acceleration bz = new Acceleration(biasZ,
3335 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3336
3337 final KnownPositionAccelerometerCalibrator calibrator =
3338 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3339 sx, sy, sz);
3340
3341
3342 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3343 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3344 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3345 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3346 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3347 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3348 final Acceleration bx2 = new Acceleration(0.0,
3349 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3350 calibrator.getInitialBiasXAsAcceleration(bx2);
3351 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3352 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3353 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3354 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3355 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3356 final Acceleration by2 = new Acceleration(0.0,
3357 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3358 calibrator.getInitialBiasYAsAcceleration(by2);
3359 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3360 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3361 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3362 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3363 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3364 final Acceleration bz2 = new Acceleration(0.0,
3365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3366 calibrator.getInitialBiasZAsAcceleration(bz2);
3367 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3368 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3369 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3370 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3371 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3372 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3373 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3374 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3375 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3376 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3377 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3378 final double[] bias1 = calibrator.getInitialBias();
3379 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3380 final double[] bias2 = new double[3];
3381 calibrator.getInitialBias(bias2);
3382 assertArrayEquals(bias1, bias2, 0.0);
3383 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3384 assertEquals(b1, ba);
3385 final Matrix b2 = new Matrix(3, 1);
3386 calibrator.getInitialBiasAsMatrix(b2);
3387 assertEquals(b1, b2);
3388 final Matrix ma1 = calibrator.getInitialMa();
3389 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3390 final Matrix ma2 = new Matrix(3, 3);
3391 calibrator.getInitialMa(ma2);
3392 assertEquals(ma1, ma2);
3393 assertNull(calibrator.getEcefPosition());
3394 assertNull(calibrator.getNedPosition());
3395 assertFalse(calibrator.getNedPosition(null));
3396 assertNull(calibrator.getMeasurements());
3397 assertFalse(calibrator.isCommonAxisUsed());
3398 assertNull(calibrator.getListener());
3399 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3400 assertFalse(calibrator.isReady());
3401 assertFalse(calibrator.isRunning());
3402 assertNull(calibrator.getEstimatedBiases());
3403 assertFalse(calibrator.getEstimatedBiases(null));
3404 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3405 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3406 assertNull(calibrator.getEstimatedBiasFx());
3407 assertNull(calibrator.getEstimatedBiasFy());
3408 assertNull(calibrator.getEstimatedBiasFz());
3409 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3410 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3411 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3412 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3413 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3414 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3415 assertNull(calibrator.getEstimatedMa());
3416 assertNull(calibrator.getEstimatedSx());
3417 assertNull(calibrator.getEstimatedSy());
3418 assertNull(calibrator.getEstimatedSz());
3419 assertNull(calibrator.getEstimatedMxy());
3420 assertNull(calibrator.getEstimatedMxz());
3421 assertNull(calibrator.getEstimatedMyx());
3422 assertNull(calibrator.getEstimatedMyz());
3423 assertNull(calibrator.getEstimatedMzx());
3424 assertNull(calibrator.getEstimatedMzy());
3425 assertNull(calibrator.getEstimatedCovariance());
3426 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3427 assertNull(calibrator.getGroundTruthGravityNorm());
3428 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3429 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3430 }
3431
3432 @Test
3433 public void testConstructor33() throws WrongSizeException {
3434 final Matrix ba = generateBa();
3435 final double biasX = ba.getElementAtIndex(0);
3436 final double biasY = ba.getElementAtIndex(1);
3437 final double biasZ = ba.getElementAtIndex(2);
3438
3439 final Matrix ma = generateMaCommonAxis();
3440 final double sx = ma.getElementAt(0, 0);
3441 final double sy = ma.getElementAt(1, 1);
3442 final double sz = ma.getElementAt(2, 2);
3443
3444 final Acceleration bx = new Acceleration(biasX,
3445 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3446 final Acceleration by = new Acceleration(biasY,
3447 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3448 final Acceleration bz = new Acceleration(biasZ,
3449 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3450
3451 final KnownPositionAccelerometerCalibrator calibrator =
3452 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3453 sx, sy, sz, this);
3454
3455
3456 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3457 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3458 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3459 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3460 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3461 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3462 final Acceleration bx2 = new Acceleration(0.0,
3463 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3464 calibrator.getInitialBiasXAsAcceleration(bx2);
3465 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3466 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3467 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3468 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3469 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3470 final Acceleration by2 = new Acceleration(0.0,
3471 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3472 calibrator.getInitialBiasYAsAcceleration(by2);
3473 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3474 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3475 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3476 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3477 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3478 final Acceleration bz2 = new Acceleration(0.0,
3479 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3480 calibrator.getInitialBiasZAsAcceleration(bz2);
3481 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3482 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3483 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3484 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3485 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3486 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3487 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3488 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3489 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3490 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3491 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3492 final double[] bias1 = calibrator.getInitialBias();
3493 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3494 final double[] bias2 = new double[3];
3495 calibrator.getInitialBias(bias2);
3496 assertArrayEquals(bias1, bias2, 0.0);
3497 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3498 assertEquals(b1, ba);
3499 final Matrix b2 = new Matrix(3, 1);
3500 calibrator.getInitialBiasAsMatrix(b2);
3501 assertEquals(b1, b2);
3502 final Matrix ma1 = calibrator.getInitialMa();
3503 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3504 final Matrix ma2 = new Matrix(3, 3);
3505 calibrator.getInitialMa(ma2);
3506 assertEquals(ma1, ma2);
3507 assertNull(calibrator.getEcefPosition());
3508 assertNull(calibrator.getNedPosition());
3509 assertFalse(calibrator.getNedPosition(null));
3510 assertNull(calibrator.getMeasurements());
3511 assertFalse(calibrator.isCommonAxisUsed());
3512 assertSame(calibrator.getListener(), this);
3513 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3514 assertFalse(calibrator.isReady());
3515 assertFalse(calibrator.isRunning());
3516 assertNull(calibrator.getEstimatedBiases());
3517 assertFalse(calibrator.getEstimatedBiases(null));
3518 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3519 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3520 assertNull(calibrator.getEstimatedBiasFx());
3521 assertNull(calibrator.getEstimatedBiasFy());
3522 assertNull(calibrator.getEstimatedBiasFz());
3523 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3524 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3525 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3526 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3527 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3528 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3529 assertNull(calibrator.getEstimatedMa());
3530 assertNull(calibrator.getEstimatedSx());
3531 assertNull(calibrator.getEstimatedSy());
3532 assertNull(calibrator.getEstimatedSz());
3533 assertNull(calibrator.getEstimatedMxy());
3534 assertNull(calibrator.getEstimatedMxz());
3535 assertNull(calibrator.getEstimatedMyx());
3536 assertNull(calibrator.getEstimatedMyz());
3537 assertNull(calibrator.getEstimatedMzx());
3538 assertNull(calibrator.getEstimatedMzy());
3539 assertNull(calibrator.getEstimatedCovariance());
3540 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3541 assertNull(calibrator.getGroundTruthGravityNorm());
3542 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3543 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3544 }
3545
3546 @Test
3547 public void testConstructor34() throws WrongSizeException {
3548 final Collection<StandardDeviationBodyKinematics> measurements =
3549 Collections.emptyList();
3550
3551 final Matrix ba = generateBa();
3552 final double biasX = ba.getElementAtIndex(0);
3553 final double biasY = ba.getElementAtIndex(1);
3554 final double biasZ = ba.getElementAtIndex(2);
3555
3556 final Matrix ma = generateMaCommonAxis();
3557 final double sx = ma.getElementAt(0, 0);
3558 final double sy = ma.getElementAt(1, 1);
3559 final double sz = ma.getElementAt(2, 2);
3560
3561 final Acceleration bx = new Acceleration(biasX,
3562 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3563 final Acceleration by = new Acceleration(biasY,
3564 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3565 final Acceleration bz = new Acceleration(biasZ,
3566 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3567
3568 final KnownPositionAccelerometerCalibrator calibrator =
3569 new KnownPositionAccelerometerCalibrator(measurements,
3570 bx, by, bz, sx, sy, sz);
3571
3572
3573 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3574 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3575 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3576 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3577 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3578 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3579 final Acceleration bx2 = new Acceleration(0.0,
3580 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3581 calibrator.getInitialBiasXAsAcceleration(bx2);
3582 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3583 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3584 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3585 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3586 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3587 final Acceleration by2 = new Acceleration(0.0,
3588 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3589 calibrator.getInitialBiasYAsAcceleration(by2);
3590 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3591 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3592 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3593 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3594 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3595 final Acceleration bz2 = new Acceleration(0.0,
3596 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3597 calibrator.getInitialBiasZAsAcceleration(bz2);
3598 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3599 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3600 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3601 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3602 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3603 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3604 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3605 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3606 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3607 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3608 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3609 final double[] bias1 = calibrator.getInitialBias();
3610 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3611 final double[] bias2 = new double[3];
3612 calibrator.getInitialBias(bias2);
3613 assertArrayEquals(bias1, bias2, 0.0);
3614 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3615 assertEquals(b1, ba);
3616 final Matrix b2 = new Matrix(3, 1);
3617 calibrator.getInitialBiasAsMatrix(b2);
3618 assertEquals(b1, b2);
3619 final Matrix ma1 = calibrator.getInitialMa();
3620 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3621 final Matrix ma2 = new Matrix(3, 3);
3622 calibrator.getInitialMa(ma2);
3623 assertEquals(ma1, ma2);
3624 assertNull(calibrator.getEcefPosition());
3625 assertNull(calibrator.getNedPosition());
3626 assertFalse(calibrator.getNedPosition(null));
3627 assertSame(calibrator.getMeasurements(), measurements);
3628 assertFalse(calibrator.isCommonAxisUsed());
3629 assertNull(calibrator.getListener());
3630 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3631 assertFalse(calibrator.isReady());
3632 assertFalse(calibrator.isRunning());
3633 assertNull(calibrator.getEstimatedBiases());
3634 assertFalse(calibrator.getEstimatedBiases(null));
3635 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3636 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3637 assertNull(calibrator.getEstimatedBiasFx());
3638 assertNull(calibrator.getEstimatedBiasFy());
3639 assertNull(calibrator.getEstimatedBiasFz());
3640 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3641 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3642 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3643 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3644 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3645 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3646 assertNull(calibrator.getEstimatedMa());
3647 assertNull(calibrator.getEstimatedSx());
3648 assertNull(calibrator.getEstimatedSy());
3649 assertNull(calibrator.getEstimatedSz());
3650 assertNull(calibrator.getEstimatedMxy());
3651 assertNull(calibrator.getEstimatedMxz());
3652 assertNull(calibrator.getEstimatedMyx());
3653 assertNull(calibrator.getEstimatedMyz());
3654 assertNull(calibrator.getEstimatedMzx());
3655 assertNull(calibrator.getEstimatedMzy());
3656 assertNull(calibrator.getEstimatedCovariance());
3657 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3658 assertNull(calibrator.getGroundTruthGravityNorm());
3659 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3660 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3661 }
3662
3663 @Test
3664 public void testConstructor35() throws WrongSizeException {
3665 final Collection<StandardDeviationBodyKinematics> measurements =
3666 Collections.emptyList();
3667
3668 final Matrix ba = generateBa();
3669 final double biasX = ba.getElementAtIndex(0);
3670 final double biasY = ba.getElementAtIndex(1);
3671 final double biasZ = ba.getElementAtIndex(2);
3672
3673 final Matrix ma = generateMaCommonAxis();
3674 final double sx = ma.getElementAt(0, 0);
3675 final double sy = ma.getElementAt(1, 1);
3676 final double sz = ma.getElementAt(2, 2);
3677
3678 final Acceleration bx = new Acceleration(biasX,
3679 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3680 final Acceleration by = new Acceleration(biasY,
3681 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3682 final Acceleration bz = new Acceleration(biasZ,
3683 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3684
3685 final KnownPositionAccelerometerCalibrator calibrator =
3686 new KnownPositionAccelerometerCalibrator(measurements,
3687 bx, by, bz, sx, sy, sz, this);
3688
3689
3690 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3691 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3692 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3693 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3694 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3695 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3696 final Acceleration bx2 = new Acceleration(0.0,
3697 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3698 calibrator.getInitialBiasXAsAcceleration(bx2);
3699 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3700 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3701 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3702 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3703 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3704 final Acceleration by2 = new Acceleration(0.0,
3705 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3706 calibrator.getInitialBiasYAsAcceleration(by2);
3707 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3708 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3709 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3710 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3711 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3712 final Acceleration bz2 = new Acceleration(0.0,
3713 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3714 calibrator.getInitialBiasZAsAcceleration(bz2);
3715 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3716 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3717 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3718 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3719 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3720 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3721 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3722 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3723 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3724 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3725 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3726 final double[] bias1 = calibrator.getInitialBias();
3727 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3728 final double[] bias2 = new double[3];
3729 calibrator.getInitialBias(bias2);
3730 assertArrayEquals(bias1, bias2, 0.0);
3731 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3732 assertEquals(b1, ba);
3733 final Matrix b2 = new Matrix(3, 1);
3734 calibrator.getInitialBiasAsMatrix(b2);
3735 assertEquals(b1, b2);
3736 final Matrix ma1 = calibrator.getInitialMa();
3737 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3738 final Matrix ma2 = new Matrix(3, 3);
3739 calibrator.getInitialMa(ma2);
3740 assertEquals(ma1, ma2);
3741 assertNull(calibrator.getEcefPosition());
3742 assertNull(calibrator.getNedPosition());
3743 assertFalse(calibrator.getNedPosition(null));
3744 assertSame(calibrator.getMeasurements(), measurements);
3745 assertFalse(calibrator.isCommonAxisUsed());
3746 assertSame(calibrator.getListener(), this);
3747 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3748 assertFalse(calibrator.isReady());
3749 assertFalse(calibrator.isRunning());
3750 assertNull(calibrator.getEstimatedBiases());
3751 assertFalse(calibrator.getEstimatedBiases(null));
3752 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3753 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3754 assertNull(calibrator.getEstimatedBiasFx());
3755 assertNull(calibrator.getEstimatedBiasFy());
3756 assertNull(calibrator.getEstimatedBiasFz());
3757 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3758 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3759 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3760 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3761 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3762 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3763 assertNull(calibrator.getEstimatedMa());
3764 assertNull(calibrator.getEstimatedSx());
3765 assertNull(calibrator.getEstimatedSy());
3766 assertNull(calibrator.getEstimatedSz());
3767 assertNull(calibrator.getEstimatedMxy());
3768 assertNull(calibrator.getEstimatedMxz());
3769 assertNull(calibrator.getEstimatedMyx());
3770 assertNull(calibrator.getEstimatedMyz());
3771 assertNull(calibrator.getEstimatedMzx());
3772 assertNull(calibrator.getEstimatedMzy());
3773 assertNull(calibrator.getEstimatedCovariance());
3774 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3775 assertNull(calibrator.getGroundTruthGravityNorm());
3776 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3777 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3778 }
3779
3780 @Test
3781 public void testConstructor36() throws WrongSizeException {
3782 final Matrix ba = generateBa();
3783 final double biasX = ba.getElementAtIndex(0);
3784 final double biasY = ba.getElementAtIndex(1);
3785 final double biasZ = ba.getElementAtIndex(2);
3786
3787 final Matrix ma = generateMaCommonAxis();
3788 final double sx = ma.getElementAt(0, 0);
3789 final double sy = ma.getElementAt(1, 1);
3790 final double sz = ma.getElementAt(2, 2);
3791
3792 final Acceleration bx = new Acceleration(biasX,
3793 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3794 final Acceleration by = new Acceleration(biasY,
3795 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3796 final Acceleration bz = new Acceleration(biasZ,
3797 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3798
3799 final KnownPositionAccelerometerCalibrator calibrator =
3800 new KnownPositionAccelerometerCalibrator(true,
3801 bx, by, bz, sx, sy, sz);
3802
3803
3804 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3805 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3806 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3807 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3808 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3809 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3810 final Acceleration bx2 = new Acceleration(0.0,
3811 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3812 calibrator.getInitialBiasXAsAcceleration(bx2);
3813 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3814 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3815 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3816 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3817 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3818 final Acceleration by2 = new Acceleration(0.0,
3819 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3820 calibrator.getInitialBiasYAsAcceleration(by2);
3821 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3822 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3823 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3824 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3825 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3826 final Acceleration bz2 = new Acceleration(0.0,
3827 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3828 calibrator.getInitialBiasZAsAcceleration(bz2);
3829 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3830 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3831 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3832 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3833 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3834 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3835 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3836 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3837 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3838 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3839 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3840 final double[] bias1 = calibrator.getInitialBias();
3841 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3842 final double[] bias2 = new double[3];
3843 calibrator.getInitialBias(bias2);
3844 assertArrayEquals(bias1, bias2, 0.0);
3845 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3846 assertEquals(b1, ba);
3847 final Matrix b2 = new Matrix(3, 1);
3848 calibrator.getInitialBiasAsMatrix(b2);
3849 assertEquals(b1, b2);
3850 final Matrix ma1 = calibrator.getInitialMa();
3851 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3852 final Matrix ma2 = new Matrix(3, 3);
3853 calibrator.getInitialMa(ma2);
3854 assertEquals(ma1, ma2);
3855 assertNull(calibrator.getEcefPosition());
3856 assertNull(calibrator.getNedPosition());
3857 assertFalse(calibrator.getNedPosition(null));
3858 assertNull(calibrator.getMeasurements());
3859 assertTrue(calibrator.isCommonAxisUsed());
3860 assertNull(calibrator.getListener());
3861 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3862 assertFalse(calibrator.isReady());
3863 assertFalse(calibrator.isRunning());
3864 assertNull(calibrator.getEstimatedBiases());
3865 assertFalse(calibrator.getEstimatedBiases(null));
3866 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3867 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3868 assertNull(calibrator.getEstimatedBiasFx());
3869 assertNull(calibrator.getEstimatedBiasFy());
3870 assertNull(calibrator.getEstimatedBiasFz());
3871 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3872 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3873 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3874 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3875 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3876 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3877 assertNull(calibrator.getEstimatedMa());
3878 assertNull(calibrator.getEstimatedSx());
3879 assertNull(calibrator.getEstimatedSy());
3880 assertNull(calibrator.getEstimatedSz());
3881 assertNull(calibrator.getEstimatedMxy());
3882 assertNull(calibrator.getEstimatedMxz());
3883 assertNull(calibrator.getEstimatedMyx());
3884 assertNull(calibrator.getEstimatedMyz());
3885 assertNull(calibrator.getEstimatedMzx());
3886 assertNull(calibrator.getEstimatedMzy());
3887 assertNull(calibrator.getEstimatedCovariance());
3888 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3889 assertNull(calibrator.getGroundTruthGravityNorm());
3890 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3891 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3892 }
3893
3894 @Test
3895 public void testConstructor37() throws WrongSizeException {
3896 final Matrix ba = generateBa();
3897 final double biasX = ba.getElementAtIndex(0);
3898 final double biasY = ba.getElementAtIndex(1);
3899 final double biasZ = ba.getElementAtIndex(2);
3900
3901 final Matrix ma = generateMaCommonAxis();
3902 final double sx = ma.getElementAt(0, 0);
3903 final double sy = ma.getElementAt(1, 1);
3904 final double sz = ma.getElementAt(2, 2);
3905
3906 final Acceleration bx = new Acceleration(biasX,
3907 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3908 final Acceleration by = new Acceleration(biasY,
3909 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3910 final Acceleration bz = new Acceleration(biasZ,
3911 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3912
3913 final KnownPositionAccelerometerCalibrator calibrator =
3914 new KnownPositionAccelerometerCalibrator(true,
3915 bx, by, bz, sx, sy, sz, this);
3916
3917
3918 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3919 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3920 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3921 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3922 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3923 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3924 final Acceleration bx2 = new Acceleration(0.0,
3925 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3926 calibrator.getInitialBiasXAsAcceleration(bx2);
3927 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3928 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3929 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3930 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3931 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3932 final Acceleration by2 = new Acceleration(0.0,
3933 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3934 calibrator.getInitialBiasYAsAcceleration(by2);
3935 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3936 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3937 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3938 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3939 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3940 final Acceleration bz2 = new Acceleration(0.0,
3941 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3942 calibrator.getInitialBiasZAsAcceleration(bz2);
3943 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3944 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3945 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3946 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3947 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3948 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3949 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3950 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3951 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3952 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3953 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3954 final double[] bias1 = calibrator.getInitialBias();
3955 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3956 final double[] bias2 = new double[3];
3957 calibrator.getInitialBias(bias2);
3958 assertArrayEquals(bias1, bias2, 0.0);
3959 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3960 assertEquals(b1, ba);
3961 final Matrix b2 = new Matrix(3, 1);
3962 calibrator.getInitialBiasAsMatrix(b2);
3963 assertEquals(b1, b2);
3964 final Matrix ma1 = calibrator.getInitialMa();
3965 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3966 final Matrix ma2 = new Matrix(3, 3);
3967 calibrator.getInitialMa(ma2);
3968 assertEquals(ma1, ma2);
3969 assertNull(calibrator.getEcefPosition());
3970 assertNull(calibrator.getNedPosition());
3971 assertFalse(calibrator.getNedPosition(null));
3972 assertNull(calibrator.getMeasurements());
3973 assertTrue(calibrator.isCommonAxisUsed());
3974 assertSame(calibrator.getListener(), this);
3975 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3976 assertFalse(calibrator.isReady());
3977 assertFalse(calibrator.isRunning());
3978 assertNull(calibrator.getEstimatedBiases());
3979 assertFalse(calibrator.getEstimatedBiases(null));
3980 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3981 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3982 assertNull(calibrator.getEstimatedBiasFx());
3983 assertNull(calibrator.getEstimatedBiasFy());
3984 assertNull(calibrator.getEstimatedBiasFz());
3985 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3986 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3987 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3988 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3989 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3990 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3991 assertNull(calibrator.getEstimatedMa());
3992 assertNull(calibrator.getEstimatedSx());
3993 assertNull(calibrator.getEstimatedSy());
3994 assertNull(calibrator.getEstimatedSz());
3995 assertNull(calibrator.getEstimatedMxy());
3996 assertNull(calibrator.getEstimatedMxz());
3997 assertNull(calibrator.getEstimatedMyx());
3998 assertNull(calibrator.getEstimatedMyz());
3999 assertNull(calibrator.getEstimatedMzx());
4000 assertNull(calibrator.getEstimatedMzy());
4001 assertNull(calibrator.getEstimatedCovariance());
4002 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4003 assertNull(calibrator.getGroundTruthGravityNorm());
4004 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4005 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4006 }
4007
4008 @Test
4009 public void testConstructor38() throws WrongSizeException {
4010 final Collection<StandardDeviationBodyKinematics> measurements =
4011 Collections.emptyList();
4012
4013 final Matrix ba = generateBa();
4014 final double biasX = ba.getElementAtIndex(0);
4015 final double biasY = ba.getElementAtIndex(1);
4016 final double biasZ = ba.getElementAtIndex(2);
4017
4018 final Matrix ma = generateMaCommonAxis();
4019 final double sx = ma.getElementAt(0, 0);
4020 final double sy = ma.getElementAt(1, 1);
4021 final double sz = ma.getElementAt(2, 2);
4022
4023 final Acceleration bx = new Acceleration(biasX,
4024 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4025 final Acceleration by = new Acceleration(biasY,
4026 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4027 final Acceleration bz = new Acceleration(biasZ,
4028 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4029
4030 final KnownPositionAccelerometerCalibrator calibrator =
4031 new KnownPositionAccelerometerCalibrator(measurements,
4032 true, bx, by, bz, sx, sy, sz);
4033
4034
4035 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4036 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4037 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4038 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4039 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4040 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4041 final Acceleration bx2 = new Acceleration(0.0,
4042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4043 calibrator.getInitialBiasXAsAcceleration(bx2);
4044 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4045 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4046 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4047 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4048 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4049 final Acceleration by2 = new Acceleration(0.0,
4050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4051 calibrator.getInitialBiasYAsAcceleration(by2);
4052 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4053 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4054 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4055 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4056 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4057 final Acceleration bz2 = new Acceleration(0.0,
4058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4059 calibrator.getInitialBiasZAsAcceleration(bz2);
4060 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4061 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4062 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4063 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4064 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4065 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4066 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4067 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4068 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4069 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4070 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4071 final double[] bias1 = calibrator.getInitialBias();
4072 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4073 final double[] bias2 = new double[3];
4074 calibrator.getInitialBias(bias2);
4075 assertArrayEquals(bias1, bias2, 0.0);
4076 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4077 assertEquals(b1, ba);
4078 final Matrix b2 = new Matrix(3, 1);
4079 calibrator.getInitialBiasAsMatrix(b2);
4080 assertEquals(b1, b2);
4081 final Matrix ma1 = calibrator.getInitialMa();
4082 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4083 final Matrix ma2 = new Matrix(3, 3);
4084 calibrator.getInitialMa(ma2);
4085 assertEquals(ma1, ma2);
4086 assertNull(calibrator.getEcefPosition());
4087 assertNull(calibrator.getNedPosition());
4088 assertFalse(calibrator.getNedPosition(null));
4089 assertSame(calibrator.getMeasurements(), measurements);
4090 assertTrue(calibrator.isCommonAxisUsed());
4091 assertNull(calibrator.getListener());
4092 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4093 assertFalse(calibrator.isReady());
4094 assertFalse(calibrator.isRunning());
4095 assertNull(calibrator.getEstimatedBiases());
4096 assertFalse(calibrator.getEstimatedBiases(null));
4097 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4098 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4099 assertNull(calibrator.getEstimatedBiasFx());
4100 assertNull(calibrator.getEstimatedBiasFy());
4101 assertNull(calibrator.getEstimatedBiasFz());
4102 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4103 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4104 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4105 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4106 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4107 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4108 assertNull(calibrator.getEstimatedMa());
4109 assertNull(calibrator.getEstimatedSx());
4110 assertNull(calibrator.getEstimatedSy());
4111 assertNull(calibrator.getEstimatedSz());
4112 assertNull(calibrator.getEstimatedMxy());
4113 assertNull(calibrator.getEstimatedMxz());
4114 assertNull(calibrator.getEstimatedMyx());
4115 assertNull(calibrator.getEstimatedMyz());
4116 assertNull(calibrator.getEstimatedMzx());
4117 assertNull(calibrator.getEstimatedMzy());
4118 assertNull(calibrator.getEstimatedCovariance());
4119 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4120 assertNull(calibrator.getGroundTruthGravityNorm());
4121 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4122 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4123 }
4124
4125 @Test
4126 public void testConstructor39() throws WrongSizeException {
4127 final Collection<StandardDeviationBodyKinematics> measurements =
4128 Collections.emptyList();
4129
4130 final Matrix ba = generateBa();
4131 final double biasX = ba.getElementAtIndex(0);
4132 final double biasY = ba.getElementAtIndex(1);
4133 final double biasZ = ba.getElementAtIndex(2);
4134
4135 final Matrix ma = generateMaCommonAxis();
4136 final double sx = ma.getElementAt(0, 0);
4137 final double sy = ma.getElementAt(1, 1);
4138 final double sz = ma.getElementAt(2, 2);
4139
4140 final Acceleration bx = new Acceleration(biasX,
4141 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4142 final Acceleration by = new Acceleration(biasY,
4143 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4144 final Acceleration bz = new Acceleration(biasZ,
4145 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4146
4147 final KnownPositionAccelerometerCalibrator calibrator =
4148 new KnownPositionAccelerometerCalibrator(measurements,
4149 true, bx, by, bz, sx, sy, sz,
4150 this);
4151
4152
4153 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4154 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4155 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4156 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4157 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4158 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4159 final Acceleration bx2 = new Acceleration(0.0,
4160 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4161 calibrator.getInitialBiasXAsAcceleration(bx2);
4162 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4163 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4164 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4165 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4166 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4167 final Acceleration by2 = new Acceleration(0.0,
4168 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4169 calibrator.getInitialBiasYAsAcceleration(by2);
4170 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4171 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4172 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4173 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4174 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4175 final Acceleration bz2 = new Acceleration(0.0,
4176 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4177 calibrator.getInitialBiasZAsAcceleration(bz2);
4178 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4179 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4180 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4181 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4182 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4183 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4184 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4185 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4186 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4187 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4188 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4189 final double[] bias1 = calibrator.getInitialBias();
4190 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4191 final double[] bias2 = new double[3];
4192 calibrator.getInitialBias(bias2);
4193 assertArrayEquals(bias1, bias2, 0.0);
4194 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4195 assertEquals(b1, ba);
4196 final Matrix b2 = new Matrix(3, 1);
4197 calibrator.getInitialBiasAsMatrix(b2);
4198 assertEquals(b1, b2);
4199 final Matrix ma1 = calibrator.getInitialMa();
4200 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4201 final Matrix ma2 = new Matrix(3, 3);
4202 calibrator.getInitialMa(ma2);
4203 assertEquals(ma1, ma2);
4204 assertNull(calibrator.getEcefPosition());
4205 assertNull(calibrator.getNedPosition());
4206 assertFalse(calibrator.getNedPosition(null));
4207 assertSame(calibrator.getMeasurements(), measurements);
4208 assertTrue(calibrator.isCommonAxisUsed());
4209 assertSame(calibrator.getListener(), this);
4210 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4211 assertFalse(calibrator.isReady());
4212 assertFalse(calibrator.isRunning());
4213 assertNull(calibrator.getEstimatedBiases());
4214 assertFalse(calibrator.getEstimatedBiases(null));
4215 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4216 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4217 assertNull(calibrator.getEstimatedBiasFx());
4218 assertNull(calibrator.getEstimatedBiasFy());
4219 assertNull(calibrator.getEstimatedBiasFz());
4220 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4221 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4222 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4223 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4224 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4225 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4226 assertNull(calibrator.getEstimatedMa());
4227 assertNull(calibrator.getEstimatedSx());
4228 assertNull(calibrator.getEstimatedSy());
4229 assertNull(calibrator.getEstimatedSz());
4230 assertNull(calibrator.getEstimatedMxy());
4231 assertNull(calibrator.getEstimatedMxz());
4232 assertNull(calibrator.getEstimatedMyx());
4233 assertNull(calibrator.getEstimatedMyz());
4234 assertNull(calibrator.getEstimatedMzx());
4235 assertNull(calibrator.getEstimatedMzy());
4236 assertNull(calibrator.getEstimatedCovariance());
4237 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4238 assertNull(calibrator.getGroundTruthGravityNorm());
4239 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4240 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4241 }
4242
4243 @Test
4244 public void testConstructor40() throws WrongSizeException {
4245 final Matrix ba = generateBa();
4246 final double biasX = ba.getElementAtIndex(0);
4247 final double biasY = ba.getElementAtIndex(1);
4248 final double biasZ = ba.getElementAtIndex(2);
4249
4250 final Matrix ma = generateMaCommonAxis();
4251 final double sx = ma.getElementAt(0, 0);
4252 final double sy = ma.getElementAt(1, 1);
4253 final double sz = ma.getElementAt(2, 2);
4254 final double mxy = ma.getElementAt(0, 1);
4255 final double mxz = ma.getElementAt(0, 2);
4256 final double myx = ma.getElementAt(1, 0);
4257 final double myz = ma.getElementAt(1, 2);
4258 final double mzx = ma.getElementAt(2, 0);
4259 final double mzy = ma.getElementAt(2, 1);
4260
4261 final KnownPositionAccelerometerCalibrator calibrator =
4262 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
4263 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4264
4265
4266 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4267 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4268 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4269 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4270 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4271 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4272 final Acceleration bx2 = new Acceleration(0.0,
4273 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4274 calibrator.getInitialBiasXAsAcceleration(bx2);
4275 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4276 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4277 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4278 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4279 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4280 final Acceleration by2 = new Acceleration(0.0,
4281 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4282 calibrator.getInitialBiasYAsAcceleration(by2);
4283 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4284 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4285 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4286 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4287 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4288 final Acceleration bz2 = new Acceleration(0.0,
4289 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4290 calibrator.getInitialBiasZAsAcceleration(bz2);
4291 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4292 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4293 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4294 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4295 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4296 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4297 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4298 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4299 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4300 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4301 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4302 final double[] bias1 = calibrator.getInitialBias();
4303 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4304 final double[] bias2 = new double[3];
4305 calibrator.getInitialBias(bias2);
4306 assertArrayEquals(bias1, bias2, 0.0);
4307 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4308 assertEquals(b1, ba);
4309 final Matrix b2 = new Matrix(3, 1);
4310 calibrator.getInitialBiasAsMatrix(b2);
4311 assertEquals(b1, b2);
4312 final Matrix ma1 = new Matrix(3, 3);
4313 ma1.setSubmatrix(0, 0,
4314 2, 2,
4315 new double[]{sx, myx, mzx,
4316 mxy, sy, mzy,
4317 mxz, myz, sz});
4318 assertEquals(calibrator.getInitialMa(), ma1);
4319 final Matrix ma2 = new Matrix(3, 3);
4320 calibrator.getInitialMa(ma2);
4321 assertEquals(ma1, ma2);
4322 assertNull(calibrator.getEcefPosition());
4323 assertNull(calibrator.getNedPosition());
4324 assertFalse(calibrator.getNedPosition(null));
4325 assertNull(calibrator.getMeasurements());
4326 assertFalse(calibrator.isCommonAxisUsed());
4327 assertNull(calibrator.getListener());
4328 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4329 assertFalse(calibrator.isReady());
4330 assertFalse(calibrator.isRunning());
4331 assertNull(calibrator.getEstimatedBiases());
4332 assertFalse(calibrator.getEstimatedBiases(null));
4333 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4334 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4335 assertNull(calibrator.getEstimatedBiasFx());
4336 assertNull(calibrator.getEstimatedBiasFy());
4337 assertNull(calibrator.getEstimatedBiasFz());
4338 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4339 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4340 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4341 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4342 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4343 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4344 assertNull(calibrator.getEstimatedMa());
4345 assertNull(calibrator.getEstimatedSx());
4346 assertNull(calibrator.getEstimatedSy());
4347 assertNull(calibrator.getEstimatedSz());
4348 assertNull(calibrator.getEstimatedMxy());
4349 assertNull(calibrator.getEstimatedMxz());
4350 assertNull(calibrator.getEstimatedMyx());
4351 assertNull(calibrator.getEstimatedMyz());
4352 assertNull(calibrator.getEstimatedMzx());
4353 assertNull(calibrator.getEstimatedMzy());
4354 assertNull(calibrator.getEstimatedCovariance());
4355 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4356 assertNull(calibrator.getGroundTruthGravityNorm());
4357 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4358 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4359 }
4360
4361 @Test
4362 public void testConstructor41() throws WrongSizeException {
4363 final Collection<StandardDeviationBodyKinematics> measurements =
4364 Collections.emptyList();
4365
4366 final Matrix ba = generateBa();
4367 final double biasX = ba.getElementAtIndex(0);
4368 final double biasY = ba.getElementAtIndex(1);
4369 final double biasZ = ba.getElementAtIndex(2);
4370
4371 final Matrix ma = generateMaCommonAxis();
4372 final double sx = ma.getElementAt(0, 0);
4373 final double sy = ma.getElementAt(1, 1);
4374 final double sz = ma.getElementAt(2, 2);
4375 final double mxy = ma.getElementAt(0, 1);
4376 final double mxz = ma.getElementAt(0, 2);
4377 final double myx = ma.getElementAt(1, 0);
4378 final double myz = ma.getElementAt(1, 2);
4379 final double mzx = ma.getElementAt(2, 0);
4380 final double mzy = ma.getElementAt(2, 1);
4381
4382 final KnownPositionAccelerometerCalibrator calibrator =
4383 new KnownPositionAccelerometerCalibrator(measurements,
4384 biasX, biasY, biasZ, sx, sy, sz,
4385 mxy, mxz, myx, myz, mzx, mzy);
4386
4387
4388 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4389 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4390 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4391 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4392 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4393 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4394 final Acceleration bx2 = new Acceleration(0.0,
4395 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4396 calibrator.getInitialBiasXAsAcceleration(bx2);
4397 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4398 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4399 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4400 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4401 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4402 final Acceleration by2 = new Acceleration(0.0,
4403 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4404 calibrator.getInitialBiasYAsAcceleration(by2);
4405 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4406 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4407 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4408 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4409 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4410 final Acceleration bz2 = new Acceleration(0.0,
4411 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4412 calibrator.getInitialBiasZAsAcceleration(bz2);
4413 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4414 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4415 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4416 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4417 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4418 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4419 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4420 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4421 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4422 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4423 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4424 final double[] bias1 = calibrator.getInitialBias();
4425 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4426 final double[] bias2 = new double[3];
4427 calibrator.getInitialBias(bias2);
4428 assertArrayEquals(bias1, bias2, 0.0);
4429 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4430 assertEquals(b1, ba);
4431 final Matrix b2 = new Matrix(3, 1);
4432 calibrator.getInitialBiasAsMatrix(b2);
4433 assertEquals(b1, b2);
4434 final Matrix ma1 = new Matrix(3, 3);
4435 ma1.setSubmatrix(0, 0,
4436 2, 2,
4437 new double[]{sx, myx, mzx,
4438 mxy, sy, mzy,
4439 mxz, myz, sz});
4440 assertEquals(calibrator.getInitialMa(), ma1);
4441 final Matrix ma2 = new Matrix(3, 3);
4442 calibrator.getInitialMa(ma2);
4443 assertEquals(ma1, ma2);
4444 assertNull(calibrator.getEcefPosition());
4445 assertNull(calibrator.getNedPosition());
4446 assertFalse(calibrator.getNedPosition(null));
4447 assertSame(calibrator.getMeasurements(), measurements);
4448 assertFalse(calibrator.isCommonAxisUsed());
4449 assertNull(calibrator.getListener());
4450 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4451 assertFalse(calibrator.isReady());
4452 assertFalse(calibrator.isRunning());
4453 assertNull(calibrator.getEstimatedBiases());
4454 assertFalse(calibrator.getEstimatedBiases(null));
4455 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4456 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4457 assertNull(calibrator.getEstimatedBiasFx());
4458 assertNull(calibrator.getEstimatedBiasFy());
4459 assertNull(calibrator.getEstimatedBiasFz());
4460 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4461 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4462 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4463 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4464 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4465 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4466 assertNull(calibrator.getEstimatedMa());
4467 assertNull(calibrator.getEstimatedSx());
4468 assertNull(calibrator.getEstimatedSy());
4469 assertNull(calibrator.getEstimatedSz());
4470 assertNull(calibrator.getEstimatedMxy());
4471 assertNull(calibrator.getEstimatedMxz());
4472 assertNull(calibrator.getEstimatedMyx());
4473 assertNull(calibrator.getEstimatedMyz());
4474 assertNull(calibrator.getEstimatedMzx());
4475 assertNull(calibrator.getEstimatedMzy());
4476 assertNull(calibrator.getEstimatedCovariance());
4477 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4478 assertNull(calibrator.getGroundTruthGravityNorm());
4479 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4480 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4481 }
4482
4483 @Test
4484 public void testConstructor42() throws WrongSizeException {
4485 final Collection<StandardDeviationBodyKinematics> measurements =
4486 Collections.emptyList();
4487
4488 final Matrix ba = generateBa();
4489 final double biasX = ba.getElementAtIndex(0);
4490 final double biasY = ba.getElementAtIndex(1);
4491 final double biasZ = ba.getElementAtIndex(2);
4492
4493 final Matrix ma = generateMaCommonAxis();
4494 final double sx = ma.getElementAt(0, 0);
4495 final double sy = ma.getElementAt(1, 1);
4496 final double sz = ma.getElementAt(2, 2);
4497 final double mxy = ma.getElementAt(0, 1);
4498 final double mxz = ma.getElementAt(0, 2);
4499 final double myx = ma.getElementAt(1, 0);
4500 final double myz = ma.getElementAt(1, 2);
4501 final double mzx = ma.getElementAt(2, 0);
4502 final double mzy = ma.getElementAt(2, 1);
4503
4504 final KnownPositionAccelerometerCalibrator calibrator =
4505 new KnownPositionAccelerometerCalibrator(measurements,
4506 biasX, biasY, biasZ, sx, sy, sz,
4507 mxy, mxz, myx, myz, mzx, mzy, this);
4508
4509
4510 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4511 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4512 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4513 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4514 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4515 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4516 final Acceleration bx2 = new Acceleration(0.0,
4517 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4518 calibrator.getInitialBiasXAsAcceleration(bx2);
4519 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4520 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4521 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4522 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4523 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4524 final Acceleration by2 = new Acceleration(0.0,
4525 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4526 calibrator.getInitialBiasYAsAcceleration(by2);
4527 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4528 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4529 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4530 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4531 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4532 final Acceleration bz2 = new Acceleration(0.0,
4533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4534 calibrator.getInitialBiasZAsAcceleration(bz2);
4535 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4536 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4537 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4538 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4539 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4540 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4541 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4542 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4543 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4544 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4545 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4546 final double[] bias1 = calibrator.getInitialBias();
4547 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4548 final double[] bias2 = new double[3];
4549 calibrator.getInitialBias(bias2);
4550 assertArrayEquals(bias1, bias2, 0.0);
4551 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4552 assertEquals(b1, ba);
4553 final Matrix b2 = new Matrix(3, 1);
4554 calibrator.getInitialBiasAsMatrix(b2);
4555 assertEquals(b1, b2);
4556 final Matrix ma1 = new Matrix(3, 3);
4557 ma1.setSubmatrix(0, 0,
4558 2, 2,
4559 new double[]{sx, myx, mzx,
4560 mxy, sy, mzy,
4561 mxz, myz, sz});
4562 assertEquals(calibrator.getInitialMa(), ma1);
4563 final Matrix ma2 = new Matrix(3, 3);
4564 calibrator.getInitialMa(ma2);
4565 assertEquals(ma1, ma2);
4566 assertNull(calibrator.getEcefPosition());
4567 assertNull(calibrator.getNedPosition());
4568 assertFalse(calibrator.getNedPosition(null));
4569 assertSame(calibrator.getMeasurements(), measurements);
4570 assertFalse(calibrator.isCommonAxisUsed());
4571 assertSame(calibrator.getListener(), this);
4572 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4573 assertFalse(calibrator.isReady());
4574 assertFalse(calibrator.isRunning());
4575 assertNull(calibrator.getEstimatedBiases());
4576 assertFalse(calibrator.getEstimatedBiases(null));
4577 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4578 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4579 assertNull(calibrator.getEstimatedBiasFx());
4580 assertNull(calibrator.getEstimatedBiasFy());
4581 assertNull(calibrator.getEstimatedBiasFz());
4582 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4583 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4584 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4585 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4586 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4587 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4588 assertNull(calibrator.getEstimatedMa());
4589 assertNull(calibrator.getEstimatedSx());
4590 assertNull(calibrator.getEstimatedSy());
4591 assertNull(calibrator.getEstimatedSz());
4592 assertNull(calibrator.getEstimatedMxy());
4593 assertNull(calibrator.getEstimatedMxz());
4594 assertNull(calibrator.getEstimatedMyx());
4595 assertNull(calibrator.getEstimatedMyz());
4596 assertNull(calibrator.getEstimatedMzx());
4597 assertNull(calibrator.getEstimatedMzy());
4598 assertNull(calibrator.getEstimatedCovariance());
4599 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4600 assertNull(calibrator.getGroundTruthGravityNorm());
4601 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4602 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4603 }
4604
4605 @Test
4606 public void testConstructor43() throws WrongSizeException {
4607 final Matrix ba = generateBa();
4608 final double biasX = ba.getElementAtIndex(0);
4609 final double biasY = ba.getElementAtIndex(1);
4610 final double biasZ = ba.getElementAtIndex(2);
4611
4612 final Matrix ma = generateMaCommonAxis();
4613 final double sx = ma.getElementAt(0, 0);
4614 final double sy = ma.getElementAt(1, 1);
4615 final double sz = ma.getElementAt(2, 2);
4616 final double mxy = ma.getElementAt(0, 1);
4617 final double mxz = ma.getElementAt(0, 2);
4618 final double myx = ma.getElementAt(1, 0);
4619 final double myz = ma.getElementAt(1, 2);
4620 final double mzx = ma.getElementAt(2, 0);
4621 final double mzy = ma.getElementAt(2, 1);
4622
4623 final KnownPositionAccelerometerCalibrator calibrator =
4624 new KnownPositionAccelerometerCalibrator(true,
4625 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4626 myz, mzx, mzy);
4627
4628
4629 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4630 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4631 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4632 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4633 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4634 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4635 final Acceleration bx2 = new Acceleration(0.0,
4636 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4637 calibrator.getInitialBiasXAsAcceleration(bx2);
4638 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4639 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4640 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4641 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4642 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4643 final Acceleration by2 = new Acceleration(0.0,
4644 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4645 calibrator.getInitialBiasYAsAcceleration(by2);
4646 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4647 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4648 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4649 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4650 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4651 final Acceleration bz2 = new Acceleration(0.0,
4652 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4653 calibrator.getInitialBiasZAsAcceleration(bz2);
4654 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4655 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4656 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4657 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4658 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4659 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4660 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4661 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4662 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4663 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4664 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4665 final double[] bias1 = calibrator.getInitialBias();
4666 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4667 final double[] bias2 = new double[3];
4668 calibrator.getInitialBias(bias2);
4669 assertArrayEquals(bias1, bias2, 0.0);
4670 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4671 assertEquals(b1, ba);
4672 final Matrix b2 = new Matrix(3, 1);
4673 calibrator.getInitialBiasAsMatrix(b2);
4674 assertEquals(b1, b2);
4675 final Matrix ma1 = new Matrix(3, 3);
4676 ma1.setSubmatrix(0, 0,
4677 2, 2,
4678 new double[]{sx, myx, mzx,
4679 mxy, sy, mzy,
4680 mxz, myz, sz});
4681 assertEquals(calibrator.getInitialMa(), ma1);
4682 final Matrix ma2 = new Matrix(3, 3);
4683 calibrator.getInitialMa(ma2);
4684 assertEquals(ma1, ma2);
4685 assertNull(calibrator.getEcefPosition());
4686 assertNull(calibrator.getNedPosition());
4687 assertFalse(calibrator.getNedPosition(null));
4688 assertNull(calibrator.getMeasurements());
4689 assertTrue(calibrator.isCommonAxisUsed());
4690 assertNull(calibrator.getListener());
4691 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4692 assertFalse(calibrator.isReady());
4693 assertFalse(calibrator.isRunning());
4694 assertNull(calibrator.getEstimatedBiases());
4695 assertFalse(calibrator.getEstimatedBiases(null));
4696 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4697 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4698 assertNull(calibrator.getEstimatedBiasFx());
4699 assertNull(calibrator.getEstimatedBiasFy());
4700 assertNull(calibrator.getEstimatedBiasFz());
4701 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4702 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4703 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4704 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4705 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4706 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4707 assertNull(calibrator.getEstimatedMa());
4708 assertNull(calibrator.getEstimatedSx());
4709 assertNull(calibrator.getEstimatedSy());
4710 assertNull(calibrator.getEstimatedSz());
4711 assertNull(calibrator.getEstimatedMxy());
4712 assertNull(calibrator.getEstimatedMxz());
4713 assertNull(calibrator.getEstimatedMyx());
4714 assertNull(calibrator.getEstimatedMyz());
4715 assertNull(calibrator.getEstimatedMzx());
4716 assertNull(calibrator.getEstimatedMzy());
4717 assertNull(calibrator.getEstimatedCovariance());
4718 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4719 assertNull(calibrator.getGroundTruthGravityNorm());
4720 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4721 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4722 }
4723
4724 @Test
4725 public void testConstructor44() throws WrongSizeException {
4726 final Matrix ba = generateBa();
4727 final double biasX = ba.getElementAtIndex(0);
4728 final double biasY = ba.getElementAtIndex(1);
4729 final double biasZ = ba.getElementAtIndex(2);
4730
4731 final Matrix ma = generateMaCommonAxis();
4732 final double sx = ma.getElementAt(0, 0);
4733 final double sy = ma.getElementAt(1, 1);
4734 final double sz = ma.getElementAt(2, 2);
4735 final double mxy = ma.getElementAt(0, 1);
4736 final double mxz = ma.getElementAt(0, 2);
4737 final double myx = ma.getElementAt(1, 0);
4738 final double myz = ma.getElementAt(1, 2);
4739 final double mzx = ma.getElementAt(2, 0);
4740 final double mzy = ma.getElementAt(2, 1);
4741
4742 final KnownPositionAccelerometerCalibrator calibrator =
4743 new KnownPositionAccelerometerCalibrator(true,
4744 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4745 myz, mzx, mzy, this);
4746
4747
4748 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4749 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4750 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4751 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4752 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4753 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4754 final Acceleration bx2 = new Acceleration(0.0,
4755 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4756 calibrator.getInitialBiasXAsAcceleration(bx2);
4757 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4758 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4759 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4760 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4761 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4762 final Acceleration by2 = new Acceleration(0.0,
4763 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4764 calibrator.getInitialBiasYAsAcceleration(by2);
4765 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4766 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4767 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4768 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4769 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4770 final Acceleration bz2 = new Acceleration(0.0,
4771 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4772 calibrator.getInitialBiasZAsAcceleration(bz2);
4773 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4774 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4775 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4776 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4777 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4778 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4779 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4780 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4781 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4782 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4783 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4784 final double[] bias1 = calibrator.getInitialBias();
4785 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4786 final double[] bias2 = new double[3];
4787 calibrator.getInitialBias(bias2);
4788 assertArrayEquals(bias1, bias2, 0.0);
4789 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4790 assertEquals(b1, ba);
4791 final Matrix b2 = new Matrix(3, 1);
4792 calibrator.getInitialBiasAsMatrix(b2);
4793 assertEquals(b1, b2);
4794 final Matrix ma1 = new Matrix(3, 3);
4795 ma1.setSubmatrix(0, 0,
4796 2, 2,
4797 new double[]{sx, myx, mzx,
4798 mxy, sy, mzy,
4799 mxz, myz, sz});
4800 assertEquals(calibrator.getInitialMa(), ma1);
4801 final Matrix ma2 = new Matrix(3, 3);
4802 calibrator.getInitialMa(ma2);
4803 assertEquals(ma1, ma2);
4804 assertNull(calibrator.getEcefPosition());
4805 assertNull(calibrator.getNedPosition());
4806 assertFalse(calibrator.getNedPosition(null));
4807 assertNull(calibrator.getMeasurements());
4808 assertTrue(calibrator.isCommonAxisUsed());
4809 assertSame(calibrator.getListener(), this);
4810 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4811 assertFalse(calibrator.isReady());
4812 assertFalse(calibrator.isRunning());
4813 assertNull(calibrator.getEstimatedBiases());
4814 assertFalse(calibrator.getEstimatedBiases(null));
4815 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4816 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4817 assertNull(calibrator.getEstimatedBiasFx());
4818 assertNull(calibrator.getEstimatedBiasFy());
4819 assertNull(calibrator.getEstimatedBiasFz());
4820 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4821 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4822 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4823 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4824 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4825 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4826 assertNull(calibrator.getEstimatedMa());
4827 assertNull(calibrator.getEstimatedSx());
4828 assertNull(calibrator.getEstimatedSy());
4829 assertNull(calibrator.getEstimatedSz());
4830 assertNull(calibrator.getEstimatedMxy());
4831 assertNull(calibrator.getEstimatedMxz());
4832 assertNull(calibrator.getEstimatedMyx());
4833 assertNull(calibrator.getEstimatedMyz());
4834 assertNull(calibrator.getEstimatedMzx());
4835 assertNull(calibrator.getEstimatedMzy());
4836 assertNull(calibrator.getEstimatedCovariance());
4837 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4838 assertNull(calibrator.getGroundTruthGravityNorm());
4839 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4840 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4841 }
4842
4843 @Test
4844 public void testConstructor45() throws WrongSizeException {
4845 final Collection<StandardDeviationBodyKinematics> measurements =
4846 Collections.emptyList();
4847
4848 final Matrix ba = generateBa();
4849 final double biasX = ba.getElementAtIndex(0);
4850 final double biasY = ba.getElementAtIndex(1);
4851 final double biasZ = ba.getElementAtIndex(2);
4852
4853 final Matrix ma = generateMaCommonAxis();
4854 final double sx = ma.getElementAt(0, 0);
4855 final double sy = ma.getElementAt(1, 1);
4856 final double sz = ma.getElementAt(2, 2);
4857 final double mxy = ma.getElementAt(0, 1);
4858 final double mxz = ma.getElementAt(0, 2);
4859 final double myx = ma.getElementAt(1, 0);
4860 final double myz = ma.getElementAt(1, 2);
4861 final double mzx = ma.getElementAt(2, 0);
4862 final double mzy = ma.getElementAt(2, 1);
4863
4864 final KnownPositionAccelerometerCalibrator calibrator =
4865 new KnownPositionAccelerometerCalibrator(measurements,
4866 true, biasX, biasY, biasZ,
4867 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4868
4869
4870 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4871 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4872 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4873 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4874 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4875 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4876 final Acceleration bx2 = new Acceleration(0.0,
4877 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4878 calibrator.getInitialBiasXAsAcceleration(bx2);
4879 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4880 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4881 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4882 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4883 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4884 final Acceleration by2 = new Acceleration(0.0,
4885 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4886 calibrator.getInitialBiasYAsAcceleration(by2);
4887 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4888 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4889 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4890 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4891 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4892 final Acceleration bz2 = new Acceleration(0.0,
4893 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4894 calibrator.getInitialBiasZAsAcceleration(bz2);
4895 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4896 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4897 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4898 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4899 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4900 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4901 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4902 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4903 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4904 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4905 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4906 final double[] bias1 = calibrator.getInitialBias();
4907 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4908 final double[] bias2 = new double[3];
4909 calibrator.getInitialBias(bias2);
4910 assertArrayEquals(bias1, bias2, 0.0);
4911 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4912 assertEquals(b1, ba);
4913 final Matrix b2 = new Matrix(3, 1);
4914 calibrator.getInitialBiasAsMatrix(b2);
4915 assertEquals(b1, b2);
4916 final Matrix ma1 = new Matrix(3, 3);
4917 ma1.setSubmatrix(0, 0,
4918 2, 2,
4919 new double[]{sx, myx, mzx,
4920 mxy, sy, mzy,
4921 mxz, myz, sz});
4922 assertEquals(calibrator.getInitialMa(), ma1);
4923 final Matrix ma2 = new Matrix(3, 3);
4924 calibrator.getInitialMa(ma2);
4925 assertEquals(ma1, ma2);
4926 assertNull(calibrator.getEcefPosition());
4927 assertNull(calibrator.getNedPosition());
4928 assertFalse(calibrator.getNedPosition(null));
4929 assertSame(calibrator.getMeasurements(), measurements);
4930 assertTrue(calibrator.isCommonAxisUsed());
4931 assertNull(calibrator.getListener());
4932 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4933 assertFalse(calibrator.isReady());
4934 assertFalse(calibrator.isRunning());
4935 assertNull(calibrator.getEstimatedBiases());
4936 assertFalse(calibrator.getEstimatedBiases(null));
4937 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4938 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4939 assertNull(calibrator.getEstimatedBiasFx());
4940 assertNull(calibrator.getEstimatedBiasFy());
4941 assertNull(calibrator.getEstimatedBiasFz());
4942 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4943 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4944 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4945 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4946 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4947 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4948 assertNull(calibrator.getEstimatedMa());
4949 assertNull(calibrator.getEstimatedSx());
4950 assertNull(calibrator.getEstimatedSy());
4951 assertNull(calibrator.getEstimatedSz());
4952 assertNull(calibrator.getEstimatedMxy());
4953 assertNull(calibrator.getEstimatedMxz());
4954 assertNull(calibrator.getEstimatedMyx());
4955 assertNull(calibrator.getEstimatedMyz());
4956 assertNull(calibrator.getEstimatedMzx());
4957 assertNull(calibrator.getEstimatedMzy());
4958 assertNull(calibrator.getEstimatedCovariance());
4959 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4960 assertNull(calibrator.getGroundTruthGravityNorm());
4961 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4962 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4963 }
4964
4965 @Test
4966 public void testConstructor46() throws WrongSizeException {
4967 final Collection<StandardDeviationBodyKinematics> measurements =
4968 Collections.emptyList();
4969
4970 final Matrix ba = generateBa();
4971 final double biasX = ba.getElementAtIndex(0);
4972 final double biasY = ba.getElementAtIndex(1);
4973 final double biasZ = ba.getElementAtIndex(2);
4974
4975 final Matrix ma = generateMaCommonAxis();
4976 final double sx = ma.getElementAt(0, 0);
4977 final double sy = ma.getElementAt(1, 1);
4978 final double sz = ma.getElementAt(2, 2);
4979 final double mxy = ma.getElementAt(0, 1);
4980 final double mxz = ma.getElementAt(0, 2);
4981 final double myx = ma.getElementAt(1, 0);
4982 final double myz = ma.getElementAt(1, 2);
4983 final double mzx = ma.getElementAt(2, 0);
4984 final double mzy = ma.getElementAt(2, 1);
4985
4986 final KnownPositionAccelerometerCalibrator calibrator =
4987 new KnownPositionAccelerometerCalibrator(measurements,
4988 true, biasX, biasY, biasZ,
4989 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4990
4991
4992 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4993 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4994 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4995 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4996 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4997 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4998 final Acceleration bx2 = new Acceleration(0.0,
4999 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5000 calibrator.getInitialBiasXAsAcceleration(bx2);
5001 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5002 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5003 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5004 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5005 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5006 final Acceleration by2 = new Acceleration(0.0,
5007 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5008 calibrator.getInitialBiasYAsAcceleration(by2);
5009 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5010 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5011 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5012 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5013 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5014 final Acceleration bz2 = new Acceleration(0.0,
5015 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5016 calibrator.getInitialBiasZAsAcceleration(bz2);
5017 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5018 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5019 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5020 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5021 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5022 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5023 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5024 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5025 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5026 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5027 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5028 final double[] bias1 = calibrator.getInitialBias();
5029 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5030 final double[] bias2 = new double[3];
5031 calibrator.getInitialBias(bias2);
5032 assertArrayEquals(bias1, bias2, 0.0);
5033 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5034 assertEquals(b1, ba);
5035 final Matrix b2 = new Matrix(3, 1);
5036 calibrator.getInitialBiasAsMatrix(b2);
5037 assertEquals(b1, b2);
5038 final Matrix ma1 = new Matrix(3, 3);
5039 ma1.setSubmatrix(0, 0,
5040 2, 2,
5041 new double[]{sx, myx, mzx,
5042 mxy, sy, mzy,
5043 mxz, myz, sz});
5044 assertEquals(calibrator.getInitialMa(), ma1);
5045 final Matrix ma2 = new Matrix(3, 3);
5046 calibrator.getInitialMa(ma2);
5047 assertEquals(ma1, ma2);
5048 assertNull(calibrator.getEcefPosition());
5049 assertNull(calibrator.getNedPosition());
5050 assertFalse(calibrator.getNedPosition(null));
5051 assertSame(calibrator.getMeasurements(), measurements);
5052 assertTrue(calibrator.isCommonAxisUsed());
5053 assertSame(calibrator.getListener(), this);
5054 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5055 assertFalse(calibrator.isReady());
5056 assertFalse(calibrator.isRunning());
5057 assertNull(calibrator.getEstimatedBiases());
5058 assertFalse(calibrator.getEstimatedBiases(null));
5059 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5060 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5061 assertNull(calibrator.getEstimatedBiasFx());
5062 assertNull(calibrator.getEstimatedBiasFy());
5063 assertNull(calibrator.getEstimatedBiasFz());
5064 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5065 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5066 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5067 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5068 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5069 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5070 assertNull(calibrator.getEstimatedMa());
5071 assertNull(calibrator.getEstimatedSx());
5072 assertNull(calibrator.getEstimatedSy());
5073 assertNull(calibrator.getEstimatedSz());
5074 assertNull(calibrator.getEstimatedMxy());
5075 assertNull(calibrator.getEstimatedMxz());
5076 assertNull(calibrator.getEstimatedMyx());
5077 assertNull(calibrator.getEstimatedMyz());
5078 assertNull(calibrator.getEstimatedMzx());
5079 assertNull(calibrator.getEstimatedMzy());
5080 assertNull(calibrator.getEstimatedCovariance());
5081 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5082 assertNull(calibrator.getGroundTruthGravityNorm());
5083 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5084 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5085 }
5086
5087 @Test
5088 public void testConstructor47() throws WrongSizeException {
5089 final Matrix ba = generateBa();
5090 final double biasX = ba.getElementAtIndex(0);
5091 final double biasY = ba.getElementAtIndex(1);
5092 final double biasZ = ba.getElementAtIndex(2);
5093
5094 final Matrix ma = generateMaCommonAxis();
5095 final double sx = ma.getElementAt(0, 0);
5096 final double sy = ma.getElementAt(1, 1);
5097 final double sz = ma.getElementAt(2, 2);
5098 final double mxy = ma.getElementAt(0, 1);
5099 final double mxz = ma.getElementAt(0, 2);
5100 final double myx = ma.getElementAt(1, 0);
5101 final double myz = ma.getElementAt(1, 2);
5102 final double mzx = ma.getElementAt(2, 0);
5103 final double mzy = ma.getElementAt(2, 1);
5104
5105 final Acceleration bx = new Acceleration(biasX,
5106 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5107 final Acceleration by = new Acceleration(biasY,
5108 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5109 final Acceleration bz = new Acceleration(biasZ,
5110 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5111
5112 final KnownPositionAccelerometerCalibrator calibrator =
5113 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5114 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5115
5116
5117 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5118 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5119 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5120 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5121 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5122 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5123 final Acceleration bx2 = new Acceleration(0.0,
5124 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5125 calibrator.getInitialBiasXAsAcceleration(bx2);
5126 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5127 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5128 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5129 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5130 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5131 final Acceleration by2 = new Acceleration(0.0,
5132 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5133 calibrator.getInitialBiasYAsAcceleration(by2);
5134 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5135 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5136 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5137 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5138 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5139 final Acceleration bz2 = new Acceleration(0.0,
5140 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5141 calibrator.getInitialBiasZAsAcceleration(bz2);
5142 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5143 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5144 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5145 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5146 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5147 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5148 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5149 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5150 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5151 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5152 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5153 final double[] bias1 = calibrator.getInitialBias();
5154 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5155 final double[] bias2 = new double[3];
5156 calibrator.getInitialBias(bias2);
5157 assertArrayEquals(bias1, bias2, 0.0);
5158 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5159 assertEquals(b1, ba);
5160 final Matrix b2 = new Matrix(3, 1);
5161 calibrator.getInitialBiasAsMatrix(b2);
5162 assertEquals(b1, b2);
5163 final Matrix ma1 = new Matrix(3, 3);
5164 ma1.setSubmatrix(0, 0,
5165 2, 2,
5166 new double[]{sx, myx, mzx,
5167 mxy, sy, mzy,
5168 mxz, myz, sz});
5169 assertEquals(calibrator.getInitialMa(), ma1);
5170 final Matrix ma2 = new Matrix(3, 3);
5171 calibrator.getInitialMa(ma2);
5172 assertEquals(ma1, ma2);
5173 assertNull(calibrator.getEcefPosition());
5174 assertNull(calibrator.getNedPosition());
5175 assertFalse(calibrator.getNedPosition(null));
5176 assertNull(calibrator.getMeasurements());
5177 assertFalse(calibrator.isCommonAxisUsed());
5178 assertNull(calibrator.getListener());
5179 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5180 assertFalse(calibrator.isReady());
5181 assertFalse(calibrator.isRunning());
5182 assertNull(calibrator.getEstimatedBiases());
5183 assertFalse(calibrator.getEstimatedBiases(null));
5184 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5185 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5186 assertNull(calibrator.getEstimatedBiasFx());
5187 assertNull(calibrator.getEstimatedBiasFy());
5188 assertNull(calibrator.getEstimatedBiasFz());
5189 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5190 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5191 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5192 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5193 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5194 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5195 assertNull(calibrator.getEstimatedMa());
5196 assertNull(calibrator.getEstimatedSx());
5197 assertNull(calibrator.getEstimatedSy());
5198 assertNull(calibrator.getEstimatedSz());
5199 assertNull(calibrator.getEstimatedMxy());
5200 assertNull(calibrator.getEstimatedMxz());
5201 assertNull(calibrator.getEstimatedMyx());
5202 assertNull(calibrator.getEstimatedMyz());
5203 assertNull(calibrator.getEstimatedMzx());
5204 assertNull(calibrator.getEstimatedMzy());
5205 assertNull(calibrator.getEstimatedCovariance());
5206 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5207 assertNull(calibrator.getGroundTruthGravityNorm());
5208 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5209 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5210 }
5211
5212 @Test
5213 public void testConstructor48() throws WrongSizeException {
5214 final Matrix ba = generateBa();
5215 final double biasX = ba.getElementAtIndex(0);
5216 final double biasY = ba.getElementAtIndex(1);
5217 final double biasZ = ba.getElementAtIndex(2);
5218
5219 final Matrix ma = generateMaCommonAxis();
5220 final double sx = ma.getElementAt(0, 0);
5221 final double sy = ma.getElementAt(1, 1);
5222 final double sz = ma.getElementAt(2, 2);
5223 final double mxy = ma.getElementAt(0, 1);
5224 final double mxz = ma.getElementAt(0, 2);
5225 final double myx = ma.getElementAt(1, 0);
5226 final double myz = ma.getElementAt(1, 2);
5227 final double mzx = ma.getElementAt(2, 0);
5228 final double mzy = ma.getElementAt(2, 1);
5229
5230 final Acceleration bx = new Acceleration(biasX,
5231 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5232 final Acceleration by = new Acceleration(biasY,
5233 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5234 final Acceleration bz = new Acceleration(biasZ,
5235 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5236
5237 final KnownPositionAccelerometerCalibrator calibrator =
5238 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5239 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5240
5241
5242 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5243 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5244 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5245 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5246 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5247 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5248 final Acceleration bx2 = new Acceleration(0.0,
5249 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5250 calibrator.getInitialBiasXAsAcceleration(bx2);
5251 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5252 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5253 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5254 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5255 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5256 final Acceleration by2 = new Acceleration(0.0,
5257 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5258 calibrator.getInitialBiasYAsAcceleration(by2);
5259 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5260 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5261 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5262 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5263 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5264 final Acceleration bz2 = new Acceleration(0.0,
5265 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5266 calibrator.getInitialBiasZAsAcceleration(bz2);
5267 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5268 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5269 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5270 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5271 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5272 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5273 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5274 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5275 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5276 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5277 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5278 final double[] bias1 = calibrator.getInitialBias();
5279 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5280 final double[] bias2 = new double[3];
5281 calibrator.getInitialBias(bias2);
5282 assertArrayEquals(bias1, bias2, 0.0);
5283 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5284 assertEquals(b1, ba);
5285 final Matrix b2 = new Matrix(3, 1);
5286 calibrator.getInitialBiasAsMatrix(b2);
5287 assertEquals(b1, b2);
5288 final Matrix ma1 = new Matrix(3, 3);
5289 ma1.setSubmatrix(0, 0,
5290 2, 2,
5291 new double[]{sx, myx, mzx,
5292 mxy, sy, mzy,
5293 mxz, myz, sz});
5294 assertEquals(calibrator.getInitialMa(), ma1);
5295 final Matrix ma2 = new Matrix(3, 3);
5296 calibrator.getInitialMa(ma2);
5297 assertEquals(ma1, ma2);
5298 assertNull(calibrator.getEcefPosition());
5299 assertNull(calibrator.getNedPosition());
5300 assertFalse(calibrator.getNedPosition(null));
5301 assertNull(calibrator.getMeasurements());
5302 assertFalse(calibrator.isCommonAxisUsed());
5303 assertSame(calibrator.getListener(), this);
5304 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5305 assertFalse(calibrator.isReady());
5306 assertFalse(calibrator.isRunning());
5307 assertNull(calibrator.getEstimatedBiases());
5308 assertFalse(calibrator.getEstimatedBiases(null));
5309 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5310 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5311 assertNull(calibrator.getEstimatedBiasFx());
5312 assertNull(calibrator.getEstimatedBiasFy());
5313 assertNull(calibrator.getEstimatedBiasFz());
5314 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5315 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5316 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5317 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5318 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5319 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5320 assertNull(calibrator.getEstimatedMa());
5321 assertNull(calibrator.getEstimatedSx());
5322 assertNull(calibrator.getEstimatedSy());
5323 assertNull(calibrator.getEstimatedSz());
5324 assertNull(calibrator.getEstimatedMxy());
5325 assertNull(calibrator.getEstimatedMxz());
5326 assertNull(calibrator.getEstimatedMyx());
5327 assertNull(calibrator.getEstimatedMyz());
5328 assertNull(calibrator.getEstimatedMzx());
5329 assertNull(calibrator.getEstimatedMzy());
5330 assertNull(calibrator.getEstimatedCovariance());
5331 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5332 assertNull(calibrator.getGroundTruthGravityNorm());
5333 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5334 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5335 }
5336
5337 @Test
5338 public void testConstructor49() throws WrongSizeException {
5339
5340 final Collection<StandardDeviationBodyKinematics> measurements =
5341 Collections.emptyList();
5342
5343 final Matrix ba = generateBa();
5344 final double biasX = ba.getElementAtIndex(0);
5345 final double biasY = ba.getElementAtIndex(1);
5346 final double biasZ = ba.getElementAtIndex(2);
5347
5348 final Matrix ma = generateMaCommonAxis();
5349 final double sx = ma.getElementAt(0, 0);
5350 final double sy = ma.getElementAt(1, 1);
5351 final double sz = ma.getElementAt(2, 2);
5352 final double mxy = ma.getElementAt(0, 1);
5353 final double mxz = ma.getElementAt(0, 2);
5354 final double myx = ma.getElementAt(1, 0);
5355 final double myz = ma.getElementAt(1, 2);
5356 final double mzx = ma.getElementAt(2, 0);
5357 final double mzy = ma.getElementAt(2, 1);
5358
5359 final Acceleration bx = new Acceleration(biasX,
5360 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5361 final Acceleration by = new Acceleration(biasY,
5362 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5363 final Acceleration bz = new Acceleration(biasZ,
5364 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5365
5366 final KnownPositionAccelerometerCalibrator calibrator =
5367 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5368 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5369
5370
5371 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5372 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5373 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5374 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5375 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5376 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5377 final Acceleration bx2 = new Acceleration(0.0,
5378 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5379 calibrator.getInitialBiasXAsAcceleration(bx2);
5380 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5381 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5382 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5383 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5384 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5385 final Acceleration by2 = new Acceleration(0.0,
5386 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5387 calibrator.getInitialBiasYAsAcceleration(by2);
5388 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5389 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5390 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5391 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5392 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5393 final Acceleration bz2 = new Acceleration(0.0,
5394 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5395 calibrator.getInitialBiasZAsAcceleration(bz2);
5396 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5397 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5398 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5399 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5400 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5401 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5402 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5403 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5404 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5405 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5406 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5407 final double[] bias1 = calibrator.getInitialBias();
5408 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5409 final double[] bias2 = new double[3];
5410 calibrator.getInitialBias(bias2);
5411 assertArrayEquals(bias1, bias2, 0.0);
5412 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5413 assertEquals(b1, ba);
5414 final Matrix b2 = new Matrix(3, 1);
5415 calibrator.getInitialBiasAsMatrix(b2);
5416 assertEquals(b1, b2);
5417 final Matrix ma1 = new Matrix(3, 3);
5418 ma1.setSubmatrix(0, 0,
5419 2, 2,
5420 new double[]{sx, myx, mzx,
5421 mxy, sy, mzy,
5422 mxz, myz, sz});
5423 assertEquals(calibrator.getInitialMa(), ma1);
5424 final Matrix ma2 = new Matrix(3, 3);
5425 calibrator.getInitialMa(ma2);
5426 assertEquals(ma1, ma2);
5427 assertNull(calibrator.getEcefPosition());
5428 assertNull(calibrator.getNedPosition());
5429 assertFalse(calibrator.getNedPosition(null));
5430 assertSame(calibrator.getMeasurements(), measurements);
5431 assertFalse(calibrator.isCommonAxisUsed());
5432 assertNull(calibrator.getListener());
5433 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5434 assertFalse(calibrator.isReady());
5435 assertFalse(calibrator.isRunning());
5436 assertNull(calibrator.getEstimatedBiases());
5437 assertFalse(calibrator.getEstimatedBiases(null));
5438 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5439 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5440 assertNull(calibrator.getEstimatedBiasFx());
5441 assertNull(calibrator.getEstimatedBiasFy());
5442 assertNull(calibrator.getEstimatedBiasFz());
5443 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5444 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5445 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5446 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5447 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5448 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5449 assertNull(calibrator.getEstimatedMa());
5450 assertNull(calibrator.getEstimatedSx());
5451 assertNull(calibrator.getEstimatedSy());
5452 assertNull(calibrator.getEstimatedSz());
5453 assertNull(calibrator.getEstimatedMxy());
5454 assertNull(calibrator.getEstimatedMxz());
5455 assertNull(calibrator.getEstimatedMyx());
5456 assertNull(calibrator.getEstimatedMyz());
5457 assertNull(calibrator.getEstimatedMzx());
5458 assertNull(calibrator.getEstimatedMzy());
5459 assertNull(calibrator.getEstimatedCovariance());
5460 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5461 assertNull(calibrator.getGroundTruthGravityNorm());
5462 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5463 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5464 }
5465
5466 @Test
5467 public void testConstructor50() throws WrongSizeException {
5468
5469 final Collection<StandardDeviationBodyKinematics> measurements =
5470 Collections.emptyList();
5471
5472 final Matrix ba = generateBa();
5473 final double biasX = ba.getElementAtIndex(0);
5474 final double biasY = ba.getElementAtIndex(1);
5475 final double biasZ = ba.getElementAtIndex(2);
5476
5477 final Matrix ma = generateMaCommonAxis();
5478 final double sx = ma.getElementAt(0, 0);
5479 final double sy = ma.getElementAt(1, 1);
5480 final double sz = ma.getElementAt(2, 2);
5481 final double mxy = ma.getElementAt(0, 1);
5482 final double mxz = ma.getElementAt(0, 2);
5483 final double myx = ma.getElementAt(1, 0);
5484 final double myz = ma.getElementAt(1, 2);
5485 final double mzx = ma.getElementAt(2, 0);
5486 final double mzy = ma.getElementAt(2, 1);
5487
5488 final Acceleration bx = new Acceleration(biasX,
5489 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5490 final Acceleration by = new Acceleration(biasY,
5491 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5492 final Acceleration bz = new Acceleration(biasZ,
5493 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5494
5495 final KnownPositionAccelerometerCalibrator calibrator =
5496 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5497 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5498
5499
5500 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5501 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5502 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5503 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5504 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5505 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5506 final Acceleration bx2 = new Acceleration(0.0,
5507 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5508 calibrator.getInitialBiasXAsAcceleration(bx2);
5509 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5510 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5511 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5512 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5513 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5514 final Acceleration by2 = new Acceleration(0.0,
5515 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5516 calibrator.getInitialBiasYAsAcceleration(by2);
5517 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5518 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5519 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5520 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5521 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5522 final Acceleration bz2 = new Acceleration(0.0,
5523 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5524 calibrator.getInitialBiasZAsAcceleration(bz2);
5525 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5526 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5527 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5528 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5529 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5530 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5531 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5532 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5533 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5534 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5535 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5536 final double[] bias1 = calibrator.getInitialBias();
5537 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5538 final double[] bias2 = new double[3];
5539 calibrator.getInitialBias(bias2);
5540 assertArrayEquals(bias1, bias2, 0.0);
5541 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5542 assertEquals(b1, ba);
5543 final Matrix b2 = new Matrix(3, 1);
5544 calibrator.getInitialBiasAsMatrix(b2);
5545 assertEquals(b1, b2);
5546 final Matrix ma1 = new Matrix(3, 3);
5547 ma1.setSubmatrix(0, 0,
5548 2, 2,
5549 new double[]{sx, myx, mzx,
5550 mxy, sy, mzy,
5551 mxz, myz, sz});
5552 assertEquals(calibrator.getInitialMa(), ma1);
5553 final Matrix ma2 = new Matrix(3, 3);
5554 calibrator.getInitialMa(ma2);
5555 assertEquals(ma1, ma2);
5556 assertNull(calibrator.getEcefPosition());
5557 assertNull(calibrator.getNedPosition());
5558 assertFalse(calibrator.getNedPosition(null));
5559 assertSame(calibrator.getMeasurements(), measurements);
5560 assertFalse(calibrator.isCommonAxisUsed());
5561 assertSame(calibrator.getListener(), this);
5562 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5563 assertFalse(calibrator.isReady());
5564 assertFalse(calibrator.isRunning());
5565 assertNull(calibrator.getEstimatedBiases());
5566 assertFalse(calibrator.getEstimatedBiases(null));
5567 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5568 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5569 assertNull(calibrator.getEstimatedBiasFx());
5570 assertNull(calibrator.getEstimatedBiasFy());
5571 assertNull(calibrator.getEstimatedBiasFz());
5572 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5573 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5574 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5575 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5576 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5577 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5578 assertNull(calibrator.getEstimatedMa());
5579 assertNull(calibrator.getEstimatedSx());
5580 assertNull(calibrator.getEstimatedSy());
5581 assertNull(calibrator.getEstimatedSz());
5582 assertNull(calibrator.getEstimatedMxy());
5583 assertNull(calibrator.getEstimatedMxz());
5584 assertNull(calibrator.getEstimatedMyx());
5585 assertNull(calibrator.getEstimatedMyz());
5586 assertNull(calibrator.getEstimatedMzx());
5587 assertNull(calibrator.getEstimatedMzy());
5588 assertNull(calibrator.getEstimatedCovariance());
5589 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5590 assertNull(calibrator.getGroundTruthGravityNorm());
5591 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5592 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5593 }
5594
5595 @Test
5596 public void testConstructor51() throws WrongSizeException {
5597 final Matrix ba = generateBa();
5598 final double biasX = ba.getElementAtIndex(0);
5599 final double biasY = ba.getElementAtIndex(1);
5600 final double biasZ = ba.getElementAtIndex(2);
5601
5602 final Matrix ma = generateMaCommonAxis();
5603 final double sx = ma.getElementAt(0, 0);
5604 final double sy = ma.getElementAt(1, 1);
5605 final double sz = ma.getElementAt(2, 2);
5606 final double mxy = ma.getElementAt(0, 1);
5607 final double mxz = ma.getElementAt(0, 2);
5608 final double myx = ma.getElementAt(1, 0);
5609 final double myz = ma.getElementAt(1, 2);
5610 final double mzx = ma.getElementAt(2, 0);
5611 final double mzy = ma.getElementAt(2, 1);
5612
5613 final Acceleration bx = new Acceleration(biasX,
5614 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5615 final Acceleration by = new Acceleration(biasY,
5616 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5617 final Acceleration bz = new Acceleration(biasZ,
5618 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5619
5620 final KnownPositionAccelerometerCalibrator calibrator =
5621 new KnownPositionAccelerometerCalibrator(true,
5622 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5623
5624
5625 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5626 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5627 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5628 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5629 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5630 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5631 final Acceleration bx2 = new Acceleration(0.0,
5632 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5633 calibrator.getInitialBiasXAsAcceleration(bx2);
5634 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5635 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5636 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5637 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5638 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5639 final Acceleration by2 = new Acceleration(0.0,
5640 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5641 calibrator.getInitialBiasYAsAcceleration(by2);
5642 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5643 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5644 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5645 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5646 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5647 final Acceleration bz2 = new Acceleration(0.0,
5648 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5649 calibrator.getInitialBiasZAsAcceleration(bz2);
5650 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5651 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5652 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5653 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5654 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5655 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5656 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5657 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5658 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5659 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5660 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5661 final double[] bias1 = calibrator.getInitialBias();
5662 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5663 final double[] bias2 = new double[3];
5664 calibrator.getInitialBias(bias2);
5665 assertArrayEquals(bias1, bias2, 0.0);
5666 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5667 assertEquals(b1, ba);
5668 final Matrix b2 = new Matrix(3, 1);
5669 calibrator.getInitialBiasAsMatrix(b2);
5670 assertEquals(b1, b2);
5671 final Matrix ma1 = new Matrix(3, 3);
5672 ma1.setSubmatrix(0, 0,
5673 2, 2,
5674 new double[]{sx, myx, mzx,
5675 mxy, sy, mzy,
5676 mxz, myz, sz});
5677 assertEquals(calibrator.getInitialMa(), ma1);
5678 final Matrix ma2 = new Matrix(3, 3);
5679 calibrator.getInitialMa(ma2);
5680 assertEquals(ma1, ma2);
5681 assertNull(calibrator.getEcefPosition());
5682 assertNull(calibrator.getNedPosition());
5683 assertFalse(calibrator.getNedPosition(null));
5684 assertNull(calibrator.getMeasurements());
5685 assertTrue(calibrator.isCommonAxisUsed());
5686 assertNull(calibrator.getListener());
5687 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5688 assertFalse(calibrator.isReady());
5689 assertFalse(calibrator.isRunning());
5690 assertNull(calibrator.getEstimatedBiases());
5691 assertFalse(calibrator.getEstimatedBiases(null));
5692 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5693 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5694 assertNull(calibrator.getEstimatedBiasFx());
5695 assertNull(calibrator.getEstimatedBiasFy());
5696 assertNull(calibrator.getEstimatedBiasFz());
5697 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5698 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5699 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5700 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5701 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5702 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5703 assertNull(calibrator.getEstimatedMa());
5704 assertNull(calibrator.getEstimatedSx());
5705 assertNull(calibrator.getEstimatedSy());
5706 assertNull(calibrator.getEstimatedSz());
5707 assertNull(calibrator.getEstimatedMxy());
5708 assertNull(calibrator.getEstimatedMxz());
5709 assertNull(calibrator.getEstimatedMyx());
5710 assertNull(calibrator.getEstimatedMyz());
5711 assertNull(calibrator.getEstimatedMzx());
5712 assertNull(calibrator.getEstimatedMzy());
5713 assertNull(calibrator.getEstimatedCovariance());
5714 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5715 assertNull(calibrator.getGroundTruthGravityNorm());
5716 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5717 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5718 }
5719
5720 @Test
5721 public void testConstructor52() throws WrongSizeException {
5722 final Matrix ba = generateBa();
5723 final double biasX = ba.getElementAtIndex(0);
5724 final double biasY = ba.getElementAtIndex(1);
5725 final double biasZ = ba.getElementAtIndex(2);
5726
5727 final Matrix ma = generateMaCommonAxis();
5728 final double sx = ma.getElementAt(0, 0);
5729 final double sy = ma.getElementAt(1, 1);
5730 final double sz = ma.getElementAt(2, 2);
5731 final double mxy = ma.getElementAt(0, 1);
5732 final double mxz = ma.getElementAt(0, 2);
5733 final double myx = ma.getElementAt(1, 0);
5734 final double myz = ma.getElementAt(1, 2);
5735 final double mzx = ma.getElementAt(2, 0);
5736 final double mzy = ma.getElementAt(2, 1);
5737
5738 final Acceleration bx = new Acceleration(biasX,
5739 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5740 final Acceleration by = new Acceleration(biasY,
5741 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5742 final Acceleration bz = new Acceleration(biasZ,
5743 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5744
5745 final KnownPositionAccelerometerCalibrator calibrator =
5746 new KnownPositionAccelerometerCalibrator(true,
5747 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5748 this);
5749
5750
5751 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5752 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5753 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5754 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5755 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5756 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5757 final Acceleration bx2 = new Acceleration(0.0,
5758 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5759 calibrator.getInitialBiasXAsAcceleration(bx2);
5760 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5761 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5762 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5763 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5764 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5765 final Acceleration by2 = new Acceleration(0.0,
5766 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5767 calibrator.getInitialBiasYAsAcceleration(by2);
5768 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5769 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5770 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5771 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5772 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5773 final Acceleration bz2 = new Acceleration(0.0,
5774 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5775 calibrator.getInitialBiasZAsAcceleration(bz2);
5776 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5777 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5778 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5779 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5780 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5781 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5782 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5783 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5784 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5785 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5786 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5787 final double[] bias1 = calibrator.getInitialBias();
5788 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5789 final double[] bias2 = new double[3];
5790 calibrator.getInitialBias(bias2);
5791 assertArrayEquals(bias1, bias2, 0.0);
5792 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5793 assertEquals(b1, ba);
5794 final Matrix b2 = new Matrix(3, 1);
5795 calibrator.getInitialBiasAsMatrix(b2);
5796 assertEquals(b1, b2);
5797 final Matrix ma1 = new Matrix(3, 3);
5798 ma1.setSubmatrix(0, 0,
5799 2, 2,
5800 new double[]{sx, myx, mzx,
5801 mxy, sy, mzy,
5802 mxz, myz, sz});
5803 assertEquals(calibrator.getInitialMa(), ma1);
5804 final Matrix ma2 = new Matrix(3, 3);
5805 calibrator.getInitialMa(ma2);
5806 assertEquals(ma1, ma2);
5807 assertNull(calibrator.getEcefPosition());
5808 assertNull(calibrator.getNedPosition());
5809 assertFalse(calibrator.getNedPosition(null));
5810 assertNull(calibrator.getMeasurements());
5811 assertTrue(calibrator.isCommonAxisUsed());
5812 assertSame(calibrator.getListener(), this);
5813 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5814 assertFalse(calibrator.isReady());
5815 assertFalse(calibrator.isRunning());
5816 assertNull(calibrator.getEstimatedBiases());
5817 assertFalse(calibrator.getEstimatedBiases(null));
5818 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5819 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5820 assertNull(calibrator.getEstimatedBiasFx());
5821 assertNull(calibrator.getEstimatedBiasFy());
5822 assertNull(calibrator.getEstimatedBiasFz());
5823 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5824 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5825 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5826 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5827 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5828 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5829 assertNull(calibrator.getEstimatedMa());
5830 assertNull(calibrator.getEstimatedSx());
5831 assertNull(calibrator.getEstimatedSy());
5832 assertNull(calibrator.getEstimatedSz());
5833 assertNull(calibrator.getEstimatedMxy());
5834 assertNull(calibrator.getEstimatedMxz());
5835 assertNull(calibrator.getEstimatedMyx());
5836 assertNull(calibrator.getEstimatedMyz());
5837 assertNull(calibrator.getEstimatedMzx());
5838 assertNull(calibrator.getEstimatedMzy());
5839 assertNull(calibrator.getEstimatedCovariance());
5840 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5841 assertNull(calibrator.getGroundTruthGravityNorm());
5842 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5843 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5844 }
5845
5846 @Test
5847 public void testConstructor53() throws WrongSizeException {
5848 final Collection<StandardDeviationBodyKinematics> measurements =
5849 Collections.emptyList();
5850
5851 final Matrix ba = generateBa();
5852 final double biasX = ba.getElementAtIndex(0);
5853 final double biasY = ba.getElementAtIndex(1);
5854 final double biasZ = ba.getElementAtIndex(2);
5855
5856 final Matrix ma = generateMaCommonAxis();
5857 final double sx = ma.getElementAt(0, 0);
5858 final double sy = ma.getElementAt(1, 1);
5859 final double sz = ma.getElementAt(2, 2);
5860 final double mxy = ma.getElementAt(0, 1);
5861 final double mxz = ma.getElementAt(0, 2);
5862 final double myx = ma.getElementAt(1, 0);
5863 final double myz = ma.getElementAt(1, 2);
5864 final double mzx = ma.getElementAt(2, 0);
5865 final double mzy = ma.getElementAt(2, 1);
5866
5867 final Acceleration bx = new Acceleration(biasX,
5868 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5869 final Acceleration by = new Acceleration(biasY,
5870 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5871 final Acceleration bz = new Acceleration(biasZ,
5872 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5873
5874 final KnownPositionAccelerometerCalibrator calibrator =
5875 new KnownPositionAccelerometerCalibrator(measurements,
5876 true, bx, by, bz, sx, sy, sz,
5877 mxy, mxz, myx, myz, mzx, mzy);
5878
5879
5880 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5881 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5882 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5883 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5884 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5885 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5886 final Acceleration bx2 = new Acceleration(0.0,
5887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5888 calibrator.getInitialBiasXAsAcceleration(bx2);
5889 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5890 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5891 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5892 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5893 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5894 final Acceleration by2 = new Acceleration(0.0,
5895 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5896 calibrator.getInitialBiasYAsAcceleration(by2);
5897 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5898 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5899 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5900 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5901 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5902 final Acceleration bz2 = new Acceleration(0.0,
5903 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5904 calibrator.getInitialBiasZAsAcceleration(bz2);
5905 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5906 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5907 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5908 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5909 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5910 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5911 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5912 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5913 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5914 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5915 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5916 final double[] bias1 = calibrator.getInitialBias();
5917 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5918 final double[] bias2 = new double[3];
5919 calibrator.getInitialBias(bias2);
5920 assertArrayEquals(bias1, bias2, 0.0);
5921 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5922 assertEquals(b1, ba);
5923 final Matrix b2 = new Matrix(3, 1);
5924 calibrator.getInitialBiasAsMatrix(b2);
5925 assertEquals(b1, b2);
5926 final Matrix ma1 = new Matrix(3, 3);
5927 ma1.setSubmatrix(0, 0,
5928 2, 2,
5929 new double[]{sx, myx, mzx,
5930 mxy, sy, mzy,
5931 mxz, myz, sz});
5932 assertEquals(calibrator.getInitialMa(), ma1);
5933 final Matrix ma2 = new Matrix(3, 3);
5934 calibrator.getInitialMa(ma2);
5935 assertEquals(ma1, ma2);
5936 assertNull(calibrator.getEcefPosition());
5937 assertNull(calibrator.getNedPosition());
5938 assertFalse(calibrator.getNedPosition(null));
5939 assertSame(calibrator.getMeasurements(), measurements);
5940 assertTrue(calibrator.isCommonAxisUsed());
5941 assertNull(calibrator.getListener());
5942 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5943 assertFalse(calibrator.isReady());
5944 assertFalse(calibrator.isRunning());
5945 assertNull(calibrator.getEstimatedBiases());
5946 assertFalse(calibrator.getEstimatedBiases(null));
5947 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5948 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5949 assertNull(calibrator.getEstimatedBiasFx());
5950 assertNull(calibrator.getEstimatedBiasFy());
5951 assertNull(calibrator.getEstimatedBiasFz());
5952 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5953 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5954 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5955 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5956 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5957 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5958 assertNull(calibrator.getEstimatedMa());
5959 assertNull(calibrator.getEstimatedSx());
5960 assertNull(calibrator.getEstimatedSy());
5961 assertNull(calibrator.getEstimatedSz());
5962 assertNull(calibrator.getEstimatedMxy());
5963 assertNull(calibrator.getEstimatedMxz());
5964 assertNull(calibrator.getEstimatedMyx());
5965 assertNull(calibrator.getEstimatedMyz());
5966 assertNull(calibrator.getEstimatedMzx());
5967 assertNull(calibrator.getEstimatedMzy());
5968 assertNull(calibrator.getEstimatedCovariance());
5969 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5970 assertNull(calibrator.getGroundTruthGravityNorm());
5971 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5972 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5973 }
5974
5975 @Test
5976 public void testConstructor54() throws WrongSizeException {
5977 final Collection<StandardDeviationBodyKinematics> measurements =
5978 Collections.emptyList();
5979
5980 final Matrix ba = generateBa();
5981 final double biasX = ba.getElementAtIndex(0);
5982 final double biasY = ba.getElementAtIndex(1);
5983 final double biasZ = ba.getElementAtIndex(2);
5984
5985 final Matrix ma = generateMaCommonAxis();
5986 final double sx = ma.getElementAt(0, 0);
5987 final double sy = ma.getElementAt(1, 1);
5988 final double sz = ma.getElementAt(2, 2);
5989 final double mxy = ma.getElementAt(0, 1);
5990 final double mxz = ma.getElementAt(0, 2);
5991 final double myx = ma.getElementAt(1, 0);
5992 final double myz = ma.getElementAt(1, 2);
5993 final double mzx = ma.getElementAt(2, 0);
5994 final double mzy = ma.getElementAt(2, 1);
5995
5996 final Acceleration bx = new Acceleration(biasX,
5997 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5998 final Acceleration by = new Acceleration(biasY,
5999 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6000 final Acceleration bz = new Acceleration(biasZ,
6001 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6002
6003 final KnownPositionAccelerometerCalibrator calibrator =
6004 new KnownPositionAccelerometerCalibrator(measurements,
6005 true, bx, by, bz, sx, sy, sz,
6006 mxy, mxz, myx, myz, mzx, mzy, this);
6007
6008
6009 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6010 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6011 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6012 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6013 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6014 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6015 final Acceleration bx2 = new Acceleration(0.0,
6016 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6017 calibrator.getInitialBiasXAsAcceleration(bx2);
6018 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6019 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6020 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6021 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6022 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6023 final Acceleration by2 = new Acceleration(0.0,
6024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6025 calibrator.getInitialBiasYAsAcceleration(by2);
6026 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6027 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6028 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6029 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6030 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6031 final Acceleration bz2 = new Acceleration(0.0,
6032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6033 calibrator.getInitialBiasZAsAcceleration(bz2);
6034 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6035 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6036 assertEquals(calibrator.getInitialSx(), sx, 0.0);
6037 assertEquals(calibrator.getInitialSy(), sy, 0.0);
6038 assertEquals(calibrator.getInitialSz(), sz, 0.0);
6039 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
6040 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
6041 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
6042 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
6043 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
6044 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
6045 final double[] bias1 = calibrator.getInitialBias();
6046 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6047 final double[] bias2 = new double[3];
6048 calibrator.getInitialBias(bias2);
6049 assertArrayEquals(bias1, bias2, 0.0);
6050 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6051 assertEquals(b1, ba);
6052 final Matrix b2 = new Matrix(3, 1);
6053 calibrator.getInitialBiasAsMatrix(b2);
6054 assertEquals(b1, b2);
6055 final Matrix ma1 = new Matrix(3, 3);
6056 ma1.setSubmatrix(0, 0,
6057 2, 2,
6058 new double[]{sx, myx, mzx,
6059 mxy, sy, mzy,
6060 mxz, myz, sz});
6061 assertEquals(calibrator.getInitialMa(), ma1);
6062 final Matrix ma2 = new Matrix(3, 3);
6063 calibrator.getInitialMa(ma2);
6064 assertEquals(ma1, ma2);
6065 assertNull(calibrator.getEcefPosition());
6066 assertNull(calibrator.getNedPosition());
6067 assertFalse(calibrator.getNedPosition(null));
6068 assertSame(calibrator.getMeasurements(), measurements);
6069 assertTrue(calibrator.isCommonAxisUsed());
6070 assertSame(calibrator.getListener(), this);
6071 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6072 assertFalse(calibrator.isReady());
6073 assertFalse(calibrator.isRunning());
6074 assertNull(calibrator.getEstimatedBiases());
6075 assertFalse(calibrator.getEstimatedBiases(null));
6076 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6077 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6078 assertNull(calibrator.getEstimatedBiasFx());
6079 assertNull(calibrator.getEstimatedBiasFy());
6080 assertNull(calibrator.getEstimatedBiasFz());
6081 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6082 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6083 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6084 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6085 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6086 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6087 assertNull(calibrator.getEstimatedMa());
6088 assertNull(calibrator.getEstimatedSx());
6089 assertNull(calibrator.getEstimatedSy());
6090 assertNull(calibrator.getEstimatedSz());
6091 assertNull(calibrator.getEstimatedMxy());
6092 assertNull(calibrator.getEstimatedMxz());
6093 assertNull(calibrator.getEstimatedMyx());
6094 assertNull(calibrator.getEstimatedMyz());
6095 assertNull(calibrator.getEstimatedMzx());
6096 assertNull(calibrator.getEstimatedMzy());
6097 assertNull(calibrator.getEstimatedCovariance());
6098 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6099 assertNull(calibrator.getGroundTruthGravityNorm());
6100 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6101 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6102 }
6103
6104 @Test
6105 public void testConstructor55() throws WrongSizeException {
6106 final Matrix ba = generateBa();
6107 final double[] bias = ba.getBuffer();
6108 final double biasX = ba.getElementAtIndex(0);
6109 final double biasY = ba.getElementAtIndex(1);
6110 final double biasZ = ba.getElementAtIndex(2);
6111
6112 KnownPositionAccelerometerCalibrator calibrator =
6113 new KnownPositionAccelerometerCalibrator(bias);
6114
6115
6116 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6117 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6118 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6119 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6120 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6121 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6122 final Acceleration bx2 = new Acceleration(0.0,
6123 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6124 calibrator.getInitialBiasXAsAcceleration(bx2);
6125 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6126 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6127 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6128 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6129 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6130 final Acceleration by2 = new Acceleration(0.0,
6131 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6132 calibrator.getInitialBiasYAsAcceleration(by2);
6133 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6134 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6135 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6136 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6137 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6138 final Acceleration bz2 = new Acceleration(0.0,
6139 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6140 calibrator.getInitialBiasZAsAcceleration(bz2);
6141 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6142 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6143 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6144 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6145 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6146 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6147 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6148 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6149 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6150 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6151 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6152 final double[] bias1 = calibrator.getInitialBias();
6153 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6154 final double[] bias2 = new double[3];
6155 calibrator.getInitialBias(bias2);
6156 assertArrayEquals(bias1, bias2, 0.0);
6157 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6158 assertEquals(b1, ba);
6159 final Matrix b2 = new Matrix(3, 1);
6160 calibrator.getInitialBiasAsMatrix(b2);
6161 assertEquals(b1, b2);
6162 final Matrix ma1 = calibrator.getInitialMa();
6163 assertEquals(ma1, new Matrix(3, 3));
6164 final Matrix ma2 = new Matrix(3, 3);
6165 calibrator.getInitialMa(ma2);
6166 assertEquals(ma1, ma2);
6167 assertNull(calibrator.getEcefPosition());
6168 assertNull(calibrator.getNedPosition());
6169 assertFalse(calibrator.getNedPosition(null));
6170 assertNull(calibrator.getMeasurements());
6171 assertFalse(calibrator.isCommonAxisUsed());
6172 assertNull(calibrator.getListener());
6173 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6174 assertFalse(calibrator.isReady());
6175 assertFalse(calibrator.isRunning());
6176 assertNull(calibrator.getEstimatedBiases());
6177 assertFalse(calibrator.getEstimatedBiases(null));
6178 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6179 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6180 assertNull(calibrator.getEstimatedBiasFx());
6181 assertNull(calibrator.getEstimatedBiasFy());
6182 assertNull(calibrator.getEstimatedBiasFz());
6183 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6184 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6185 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6186 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6187 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6188 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6189 assertNull(calibrator.getEstimatedMa());
6190 assertNull(calibrator.getEstimatedSx());
6191 assertNull(calibrator.getEstimatedSy());
6192 assertNull(calibrator.getEstimatedSz());
6193 assertNull(calibrator.getEstimatedMxy());
6194 assertNull(calibrator.getEstimatedMxz());
6195 assertNull(calibrator.getEstimatedMyx());
6196 assertNull(calibrator.getEstimatedMyz());
6197 assertNull(calibrator.getEstimatedMzx());
6198 assertNull(calibrator.getEstimatedMzy());
6199 assertNull(calibrator.getEstimatedCovariance());
6200 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6201 assertNull(calibrator.getGroundTruthGravityNorm());
6202 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6203 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6204
6205
6206 calibrator = null;
6207 try {
6208 calibrator = new KnownPositionAccelerometerCalibrator(new double[1]);
6209 fail("IllegalArgumentException expected but not thrown");
6210 } catch (final IllegalArgumentException ignore) {
6211 }
6212 assertNull(calibrator);
6213 }
6214
6215 @Test
6216 public void testConstructor56() throws WrongSizeException {
6217 final Matrix ba = generateBa();
6218 final double[] bias = ba.getBuffer();
6219 final double biasX = ba.getElementAtIndex(0);
6220 final double biasY = ba.getElementAtIndex(1);
6221 final double biasZ = ba.getElementAtIndex(2);
6222
6223 KnownPositionAccelerometerCalibrator calibrator =
6224 new KnownPositionAccelerometerCalibrator(bias, this);
6225
6226
6227 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6228 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6229 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6230 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6231 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6232 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6233 final Acceleration bx2 = new Acceleration(0.0,
6234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6235 calibrator.getInitialBiasXAsAcceleration(bx2);
6236 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6237 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6238 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6239 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6240 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6241 final Acceleration by2 = new Acceleration(0.0,
6242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6243 calibrator.getInitialBiasYAsAcceleration(by2);
6244 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6245 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6246 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6247 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6248 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6249 final Acceleration bz2 = new Acceleration(0.0,
6250 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6251 calibrator.getInitialBiasZAsAcceleration(bz2);
6252 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6253 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6254 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6255 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6256 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6257 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6258 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6259 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6260 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6261 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6262 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6263 final double[] bias1 = calibrator.getInitialBias();
6264 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6265 final double[] bias2 = new double[3];
6266 calibrator.getInitialBias(bias2);
6267 assertArrayEquals(bias1, bias2, 0.0);
6268 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6269 assertEquals(b1, ba);
6270 final Matrix b2 = new Matrix(3, 1);
6271 calibrator.getInitialBiasAsMatrix(b2);
6272 assertEquals(b1, b2);
6273 final Matrix ma1 = calibrator.getInitialMa();
6274 assertEquals(ma1, new Matrix(3, 3));
6275 final Matrix ma2 = new Matrix(3, 3);
6276 calibrator.getInitialMa(ma2);
6277 assertEquals(ma1, ma2);
6278 assertNull(calibrator.getEcefPosition());
6279 assertNull(calibrator.getNedPosition());
6280 assertFalse(calibrator.getNedPosition(null));
6281 assertNull(calibrator.getMeasurements());
6282 assertFalse(calibrator.isCommonAxisUsed());
6283 assertSame(calibrator.getListener(), this);
6284 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6285 assertFalse(calibrator.isReady());
6286 assertFalse(calibrator.isRunning());
6287 assertNull(calibrator.getEstimatedBiases());
6288 assertFalse(calibrator.getEstimatedBiases(null));
6289 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6290 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6291 assertNull(calibrator.getEstimatedBiasFx());
6292 assertNull(calibrator.getEstimatedBiasFy());
6293 assertNull(calibrator.getEstimatedBiasFz());
6294 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6295 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6296 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6297 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6298 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6299 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6300 assertNull(calibrator.getEstimatedMa());
6301 assertNull(calibrator.getEstimatedSx());
6302 assertNull(calibrator.getEstimatedSy());
6303 assertNull(calibrator.getEstimatedSz());
6304 assertNull(calibrator.getEstimatedMxy());
6305 assertNull(calibrator.getEstimatedMxz());
6306 assertNull(calibrator.getEstimatedMyx());
6307 assertNull(calibrator.getEstimatedMyz());
6308 assertNull(calibrator.getEstimatedMzx());
6309 assertNull(calibrator.getEstimatedMzy());
6310 assertNull(calibrator.getEstimatedCovariance());
6311 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6312 assertNull(calibrator.getGroundTruthGravityNorm());
6313 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6314 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6315
6316
6317 calibrator = null;
6318 try {
6319 calibrator = new KnownPositionAccelerometerCalibrator(new double[1],
6320 this);
6321 fail("IllegalArgumentException expected but not thrown");
6322 } catch (final IllegalArgumentException ignore) {
6323 }
6324 assertNull(calibrator);
6325 }
6326
6327 @Test
6328 public void testConstructor57() throws WrongSizeException {
6329 final Collection<StandardDeviationBodyKinematics> measurements =
6330 Collections.emptyList();
6331
6332 final Matrix ba = generateBa();
6333 final double[] bias = ba.getBuffer();
6334 final double biasX = ba.getElementAtIndex(0);
6335 final double biasY = ba.getElementAtIndex(1);
6336 final double biasZ = ba.getElementAtIndex(2);
6337
6338 KnownPositionAccelerometerCalibrator calibrator =
6339 new KnownPositionAccelerometerCalibrator(measurements, bias);
6340
6341
6342 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6343 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6344 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6345 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6346 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6347 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6348 final Acceleration bx2 = new Acceleration(0.0,
6349 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6350 calibrator.getInitialBiasXAsAcceleration(bx2);
6351 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6352 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6353 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6354 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6355 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6356 final Acceleration by2 = new Acceleration(0.0,
6357 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6358 calibrator.getInitialBiasYAsAcceleration(by2);
6359 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6360 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6361 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6362 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6363 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6364 final Acceleration bz2 = new Acceleration(0.0,
6365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6366 calibrator.getInitialBiasZAsAcceleration(bz2);
6367 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6368 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6369 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6370 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6371 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6372 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6373 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6374 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6375 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6376 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6377 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6378 final double[] bias1 = calibrator.getInitialBias();
6379 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6380 final double[] bias2 = new double[3];
6381 calibrator.getInitialBias(bias2);
6382 assertArrayEquals(bias1, bias2, 0.0);
6383 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6384 assertEquals(b1, ba);
6385 final Matrix b2 = new Matrix(3, 1);
6386 calibrator.getInitialBiasAsMatrix(b2);
6387 assertEquals(b1, b2);
6388 final Matrix ma1 = calibrator.getInitialMa();
6389 assertEquals(ma1, new Matrix(3, 3));
6390 final Matrix ma2 = new Matrix(3, 3);
6391 calibrator.getInitialMa(ma2);
6392 assertEquals(ma1, ma2);
6393 assertNull(calibrator.getEcefPosition());
6394 assertNull(calibrator.getNedPosition());
6395 assertFalse(calibrator.getNedPosition(null));
6396 assertSame(calibrator.getMeasurements(), measurements);
6397 assertFalse(calibrator.isCommonAxisUsed());
6398 assertNull(calibrator.getListener());
6399 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6400 assertFalse(calibrator.isReady());
6401 assertFalse(calibrator.isRunning());
6402 assertNull(calibrator.getEstimatedBiases());
6403 assertFalse(calibrator.getEstimatedBiases(null));
6404 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6405 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6406 assertNull(calibrator.getEstimatedBiasFx());
6407 assertNull(calibrator.getEstimatedBiasFy());
6408 assertNull(calibrator.getEstimatedBiasFz());
6409 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6410 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6411 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6412 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6413 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6414 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6415 assertNull(calibrator.getEstimatedMa());
6416 assertNull(calibrator.getEstimatedSx());
6417 assertNull(calibrator.getEstimatedSy());
6418 assertNull(calibrator.getEstimatedSz());
6419 assertNull(calibrator.getEstimatedMxy());
6420 assertNull(calibrator.getEstimatedMxz());
6421 assertNull(calibrator.getEstimatedMyx());
6422 assertNull(calibrator.getEstimatedMyz());
6423 assertNull(calibrator.getEstimatedMzx());
6424 assertNull(calibrator.getEstimatedMzy());
6425 assertNull(calibrator.getEstimatedCovariance());
6426 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6427 assertNull(calibrator.getGroundTruthGravityNorm());
6428 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6429 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6430
6431
6432 calibrator = null;
6433 try {
6434 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6435 new double[1]);
6436 fail("IllegalArgumentException expected but not thrown");
6437 } catch (final IllegalArgumentException ignore) {
6438 }
6439 assertNull(calibrator);
6440 }
6441
6442 @Test
6443 public void testConstructor58() throws WrongSizeException {
6444 final Collection<StandardDeviationBodyKinematics> measurements =
6445 Collections.emptyList();
6446
6447 final Matrix ba = generateBa();
6448 final double[] bias = ba.getBuffer();
6449 final double biasX = ba.getElementAtIndex(0);
6450 final double biasY = ba.getElementAtIndex(1);
6451 final double biasZ = ba.getElementAtIndex(2);
6452
6453 KnownPositionAccelerometerCalibrator calibrator =
6454 new KnownPositionAccelerometerCalibrator(measurements, bias,
6455 this);
6456
6457
6458 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6459 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6460 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6461 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6462 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6463 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6464 final Acceleration bx2 = new Acceleration(0.0,
6465 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6466 calibrator.getInitialBiasXAsAcceleration(bx2);
6467 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6468 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6469 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6470 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6471 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6472 final Acceleration by2 = new Acceleration(0.0,
6473 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6474 calibrator.getInitialBiasYAsAcceleration(by2);
6475 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6476 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6477 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6478 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6479 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6480 final Acceleration bz2 = new Acceleration(0.0,
6481 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6482 calibrator.getInitialBiasZAsAcceleration(bz2);
6483 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6484 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6485 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6486 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6487 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6488 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6489 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6490 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6491 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6492 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6493 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6494 final double[] bias1 = calibrator.getInitialBias();
6495 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6496 final double[] bias2 = new double[3];
6497 calibrator.getInitialBias(bias2);
6498 assertArrayEquals(bias1, bias2, 0.0);
6499 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6500 assertEquals(b1, ba);
6501 final Matrix b2 = new Matrix(3, 1);
6502 calibrator.getInitialBiasAsMatrix(b2);
6503 assertEquals(b1, b2);
6504 final Matrix ma1 = calibrator.getInitialMa();
6505 assertEquals(ma1, new Matrix(3, 3));
6506 final Matrix ma2 = new Matrix(3, 3);
6507 calibrator.getInitialMa(ma2);
6508 assertEquals(ma1, ma2);
6509 assertNull(calibrator.getEcefPosition());
6510 assertNull(calibrator.getNedPosition());
6511 assertFalse(calibrator.getNedPosition(null));
6512 assertSame(calibrator.getMeasurements(), measurements);
6513 assertFalse(calibrator.isCommonAxisUsed());
6514 assertSame(calibrator.getListener(), this);
6515 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6516 assertFalse(calibrator.isReady());
6517 assertFalse(calibrator.isRunning());
6518 assertNull(calibrator.getEstimatedBiases());
6519 assertFalse(calibrator.getEstimatedBiases(null));
6520 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6521 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6522 assertNull(calibrator.getEstimatedBiasFx());
6523 assertNull(calibrator.getEstimatedBiasFy());
6524 assertNull(calibrator.getEstimatedBiasFz());
6525 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6526 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6527 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6528 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6529 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6530 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6531 assertNull(calibrator.getEstimatedMa());
6532 assertNull(calibrator.getEstimatedSx());
6533 assertNull(calibrator.getEstimatedSy());
6534 assertNull(calibrator.getEstimatedSz());
6535 assertNull(calibrator.getEstimatedMxy());
6536 assertNull(calibrator.getEstimatedMxz());
6537 assertNull(calibrator.getEstimatedMyx());
6538 assertNull(calibrator.getEstimatedMyz());
6539 assertNull(calibrator.getEstimatedMzx());
6540 assertNull(calibrator.getEstimatedMzy());
6541 assertNull(calibrator.getEstimatedCovariance());
6542 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6543 assertNull(calibrator.getGroundTruthGravityNorm());
6544 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6545 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6546
6547
6548 calibrator = null;
6549 try {
6550 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6551 new double[1], this);
6552 fail("IllegalArgumentException expected but not thrown");
6553 } catch (final IllegalArgumentException ignore) {
6554 }
6555 assertNull(calibrator);
6556 }
6557
6558 @Test
6559 public void testConstructor59() throws WrongSizeException {
6560 final Matrix ba = generateBa();
6561 final double[] bias = ba.getBuffer();
6562 final double biasX = ba.getElementAtIndex(0);
6563 final double biasY = ba.getElementAtIndex(1);
6564 final double biasZ = ba.getElementAtIndex(2);
6565
6566 KnownPositionAccelerometerCalibrator calibrator =
6567 new KnownPositionAccelerometerCalibrator(true, bias);
6568
6569
6570 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6571 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6572 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6573 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6574 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6575 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6576 final Acceleration bx2 = new Acceleration(0.0,
6577 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6578 calibrator.getInitialBiasXAsAcceleration(bx2);
6579 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6580 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6581 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6582 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6583 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6584 final Acceleration by2 = new Acceleration(0.0,
6585 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6586 calibrator.getInitialBiasYAsAcceleration(by2);
6587 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6588 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6589 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6590 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6591 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6592 final Acceleration bz2 = new Acceleration(0.0,
6593 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6594 calibrator.getInitialBiasZAsAcceleration(bz2);
6595 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6596 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6597 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6598 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6599 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6600 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6601 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6602 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6603 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6604 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6605 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6606 final double[] bias1 = calibrator.getInitialBias();
6607 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6608 final double[] bias2 = new double[3];
6609 calibrator.getInitialBias(bias2);
6610 assertArrayEquals(bias1, bias2, 0.0);
6611 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6612 assertEquals(b1, ba);
6613 final Matrix b2 = new Matrix(3, 1);
6614 calibrator.getInitialBiasAsMatrix(b2);
6615 assertEquals(b1, b2);
6616 final Matrix ma1 = calibrator.getInitialMa();
6617 assertEquals(ma1, new Matrix(3, 3));
6618 final Matrix ma2 = new Matrix(3, 3);
6619 calibrator.getInitialMa(ma2);
6620 assertEquals(ma1, ma2);
6621 assertNull(calibrator.getEcefPosition());
6622 assertNull(calibrator.getNedPosition());
6623 assertFalse(calibrator.getNedPosition(null));
6624 assertNull(calibrator.getMeasurements());
6625 assertTrue(calibrator.isCommonAxisUsed());
6626 assertNull(calibrator.getListener());
6627 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6628 assertFalse(calibrator.isReady());
6629 assertFalse(calibrator.isRunning());
6630 assertNull(calibrator.getEstimatedBiases());
6631 assertFalse(calibrator.getEstimatedBiases(null));
6632 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6633 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6634 assertNull(calibrator.getEstimatedBiasFx());
6635 assertNull(calibrator.getEstimatedBiasFy());
6636 assertNull(calibrator.getEstimatedBiasFz());
6637 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6638 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6639 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6640 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6641 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6642 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6643 assertNull(calibrator.getEstimatedMa());
6644 assertNull(calibrator.getEstimatedSx());
6645 assertNull(calibrator.getEstimatedSy());
6646 assertNull(calibrator.getEstimatedSz());
6647 assertNull(calibrator.getEstimatedMxy());
6648 assertNull(calibrator.getEstimatedMxz());
6649 assertNull(calibrator.getEstimatedMyx());
6650 assertNull(calibrator.getEstimatedMyz());
6651 assertNull(calibrator.getEstimatedMzx());
6652 assertNull(calibrator.getEstimatedMzy());
6653 assertNull(calibrator.getEstimatedCovariance());
6654 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6655 assertNull(calibrator.getGroundTruthGravityNorm());
6656 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6657 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6658
6659
6660 calibrator = null;
6661 try {
6662 calibrator = new KnownPositionAccelerometerCalibrator(true,
6663 new double[1]);
6664 fail("IllegalArgumentException expected but not thrown");
6665 } catch (final IllegalArgumentException ignore) {
6666 }
6667 assertNull(calibrator);
6668 }
6669
6670 @Test
6671 public void testConstructor60() throws WrongSizeException {
6672 final Matrix ba = generateBa();
6673 final double[] bias = ba.getBuffer();
6674 final double biasX = ba.getElementAtIndex(0);
6675 final double biasY = ba.getElementAtIndex(1);
6676 final double biasZ = ba.getElementAtIndex(2);
6677
6678 KnownPositionAccelerometerCalibrator calibrator =
6679 new KnownPositionAccelerometerCalibrator(true, bias, this);
6680
6681
6682 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6683 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6684 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6685 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6686 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6687 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6688 final Acceleration bx2 = new Acceleration(0.0,
6689 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6690 calibrator.getInitialBiasXAsAcceleration(bx2);
6691 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6692 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6693 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6694 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6695 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6696 final Acceleration by2 = new Acceleration(0.0,
6697 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6698 calibrator.getInitialBiasYAsAcceleration(by2);
6699 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6700 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6701 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6702 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6703 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6704 final Acceleration bz2 = new Acceleration(0.0,
6705 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6706 calibrator.getInitialBiasZAsAcceleration(bz2);
6707 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6708 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6709 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6710 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6711 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6712 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6713 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6714 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6715 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6716 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6717 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6718 final double[] bias1 = calibrator.getInitialBias();
6719 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6720 final double[] bias2 = new double[3];
6721 calibrator.getInitialBias(bias2);
6722 assertArrayEquals(bias1, bias2, 0.0);
6723 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6724 assertEquals(b1, ba);
6725 final Matrix b2 = new Matrix(3, 1);
6726 calibrator.getInitialBiasAsMatrix(b2);
6727 assertEquals(b1, b2);
6728 final Matrix ma1 = calibrator.getInitialMa();
6729 assertEquals(ma1, new Matrix(3, 3));
6730 final Matrix ma2 = new Matrix(3, 3);
6731 calibrator.getInitialMa(ma2);
6732 assertEquals(ma1, ma2);
6733 assertNull(calibrator.getEcefPosition());
6734 assertNull(calibrator.getNedPosition());
6735 assertFalse(calibrator.getNedPosition(null));
6736 assertNull(calibrator.getMeasurements());
6737 assertTrue(calibrator.isCommonAxisUsed());
6738 assertSame(calibrator.getListener(), this);
6739 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6740 assertFalse(calibrator.isReady());
6741 assertFalse(calibrator.isRunning());
6742 assertNull(calibrator.getEstimatedBiases());
6743 assertFalse(calibrator.getEstimatedBiases(null));
6744 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6745 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6746 assertNull(calibrator.getEstimatedBiasFx());
6747 assertNull(calibrator.getEstimatedBiasFy());
6748 assertNull(calibrator.getEstimatedBiasFz());
6749 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6750 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6751 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6752 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6753 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6754 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6755 assertNull(calibrator.getEstimatedMa());
6756 assertNull(calibrator.getEstimatedSx());
6757 assertNull(calibrator.getEstimatedSy());
6758 assertNull(calibrator.getEstimatedSz());
6759 assertNull(calibrator.getEstimatedMxy());
6760 assertNull(calibrator.getEstimatedMxz());
6761 assertNull(calibrator.getEstimatedMyx());
6762 assertNull(calibrator.getEstimatedMyz());
6763 assertNull(calibrator.getEstimatedMzx());
6764 assertNull(calibrator.getEstimatedMzy());
6765 assertNull(calibrator.getEstimatedCovariance());
6766 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6767 assertNull(calibrator.getGroundTruthGravityNorm());
6768 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6769 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6770
6771
6772 calibrator = null;
6773 try {
6774 calibrator = new KnownPositionAccelerometerCalibrator(true,
6775 new double[1], this);
6776 fail("IllegalArgumentException expected but not thrown");
6777 } catch (final IllegalArgumentException ignore) {
6778 }
6779 assertNull(calibrator);
6780 }
6781
6782 @Test
6783 public void testConstructor61() throws WrongSizeException {
6784 final Collection<StandardDeviationBodyKinematics> measurements =
6785 Collections.emptyList();
6786
6787 final Matrix ba = generateBa();
6788 final double[] bias = ba.getBuffer();
6789 final double biasX = ba.getElementAtIndex(0);
6790 final double biasY = ba.getElementAtIndex(1);
6791 final double biasZ = ba.getElementAtIndex(2);
6792
6793 KnownPositionAccelerometerCalibrator calibrator =
6794 new KnownPositionAccelerometerCalibrator(measurements,
6795 true, bias);
6796
6797
6798 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6799 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6800 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6801 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6802 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6803 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6804 final Acceleration bx2 = new Acceleration(0.0,
6805 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6806 calibrator.getInitialBiasXAsAcceleration(bx2);
6807 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6808 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6809 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6810 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6811 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6812 final Acceleration by2 = new Acceleration(0.0,
6813 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6814 calibrator.getInitialBiasYAsAcceleration(by2);
6815 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6816 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6817 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6818 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6819 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6820 final Acceleration bz2 = new Acceleration(0.0,
6821 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6822 calibrator.getInitialBiasZAsAcceleration(bz2);
6823 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6824 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6825 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6826 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6827 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6828 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6829 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6830 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6831 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6832 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6833 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6834 final double[] bias1 = calibrator.getInitialBias();
6835 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6836 final double[] bias2 = new double[3];
6837 calibrator.getInitialBias(bias2);
6838 assertArrayEquals(bias1, bias2, 0.0);
6839 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6840 assertEquals(b1, ba);
6841 final Matrix b2 = new Matrix(3, 1);
6842 calibrator.getInitialBiasAsMatrix(b2);
6843 assertEquals(b1, b2);
6844 final Matrix ma1 = calibrator.getInitialMa();
6845 assertEquals(ma1, new Matrix(3, 3));
6846 final Matrix ma2 = new Matrix(3, 3);
6847 calibrator.getInitialMa(ma2);
6848 assertEquals(ma1, ma2);
6849 assertNull(calibrator.getEcefPosition());
6850 assertNull(calibrator.getNedPosition());
6851 assertFalse(calibrator.getNedPosition(null));
6852 assertSame(calibrator.getMeasurements(), measurements);
6853 assertTrue(calibrator.isCommonAxisUsed());
6854 assertNull(calibrator.getListener());
6855 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6856 assertFalse(calibrator.isReady());
6857 assertFalse(calibrator.isRunning());
6858 assertNull(calibrator.getEstimatedBiases());
6859 assertFalse(calibrator.getEstimatedBiases(null));
6860 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6861 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6862 assertNull(calibrator.getEstimatedBiasFx());
6863 assertNull(calibrator.getEstimatedBiasFy());
6864 assertNull(calibrator.getEstimatedBiasFz());
6865 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6866 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6867 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6868 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6869 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6870 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6871 assertNull(calibrator.getEstimatedMa());
6872 assertNull(calibrator.getEstimatedSx());
6873 assertNull(calibrator.getEstimatedSy());
6874 assertNull(calibrator.getEstimatedSz());
6875 assertNull(calibrator.getEstimatedMxy());
6876 assertNull(calibrator.getEstimatedMxz());
6877 assertNull(calibrator.getEstimatedMyx());
6878 assertNull(calibrator.getEstimatedMyz());
6879 assertNull(calibrator.getEstimatedMzx());
6880 assertNull(calibrator.getEstimatedMzy());
6881 assertNull(calibrator.getEstimatedCovariance());
6882 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6883 assertNull(calibrator.getGroundTruthGravityNorm());
6884 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6885 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6886
6887
6888 calibrator = null;
6889 try {
6890 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6891 true, new double[1]);
6892 fail("IllegalArgumentException expected but not thrown");
6893 } catch (final IllegalArgumentException ignore) {
6894 }
6895 assertNull(calibrator);
6896 }
6897
6898 @Test
6899 public void testConstructor62() throws WrongSizeException {
6900 final Collection<StandardDeviationBodyKinematics> measurements =
6901 Collections.emptyList();
6902
6903 final Matrix ba = generateBa();
6904 final double[] bias = ba.getBuffer();
6905 final double biasX = ba.getElementAtIndex(0);
6906 final double biasY = ba.getElementAtIndex(1);
6907 final double biasZ = ba.getElementAtIndex(2);
6908
6909 KnownPositionAccelerometerCalibrator calibrator =
6910 new KnownPositionAccelerometerCalibrator(measurements,
6911 true, bias, this);
6912
6913
6914 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6915 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6916 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6917 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6918 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6919 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6920 final Acceleration bx2 = new Acceleration(0.0,
6921 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6922 calibrator.getInitialBiasXAsAcceleration(bx2);
6923 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6924 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6925 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6926 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6927 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6928 final Acceleration by2 = new Acceleration(0.0,
6929 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6930 calibrator.getInitialBiasYAsAcceleration(by2);
6931 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6932 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6933 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6934 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6935 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6936 final Acceleration bz2 = new Acceleration(0.0,
6937 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6938 calibrator.getInitialBiasZAsAcceleration(bz2);
6939 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6940 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6941 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6942 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6943 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6944 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6945 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6946 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6947 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6948 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6949 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6950 final double[] bias1 = calibrator.getInitialBias();
6951 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6952 final double[] bias2 = new double[3];
6953 calibrator.getInitialBias(bias2);
6954 assertArrayEquals(bias1, bias2, 0.0);
6955 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6956 assertEquals(b1, ba);
6957 final Matrix b2 = new Matrix(3, 1);
6958 calibrator.getInitialBiasAsMatrix(b2);
6959 assertEquals(b1, b2);
6960 final Matrix ma1 = calibrator.getInitialMa();
6961 assertEquals(ma1, new Matrix(3, 3));
6962 final Matrix ma2 = new Matrix(3, 3);
6963 calibrator.getInitialMa(ma2);
6964 assertEquals(ma1, ma2);
6965 assertNull(calibrator.getEcefPosition());
6966 assertNull(calibrator.getNedPosition());
6967 assertFalse(calibrator.getNedPosition(null));
6968 assertSame(calibrator.getMeasurements(), measurements);
6969 assertTrue(calibrator.isCommonAxisUsed());
6970 assertSame(calibrator.getListener(), this);
6971 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6972 assertFalse(calibrator.isReady());
6973 assertFalse(calibrator.isRunning());
6974 assertNull(calibrator.getEstimatedBiases());
6975 assertFalse(calibrator.getEstimatedBiases(null));
6976 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6977 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6978 assertNull(calibrator.getEstimatedBiasFx());
6979 assertNull(calibrator.getEstimatedBiasFy());
6980 assertNull(calibrator.getEstimatedBiasFz());
6981 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6982 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6983 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6984 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6985 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6986 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6987 assertNull(calibrator.getEstimatedMa());
6988 assertNull(calibrator.getEstimatedSx());
6989 assertNull(calibrator.getEstimatedSy());
6990 assertNull(calibrator.getEstimatedSz());
6991 assertNull(calibrator.getEstimatedMxy());
6992 assertNull(calibrator.getEstimatedMxz());
6993 assertNull(calibrator.getEstimatedMyx());
6994 assertNull(calibrator.getEstimatedMyz());
6995 assertNull(calibrator.getEstimatedMzx());
6996 assertNull(calibrator.getEstimatedMzy());
6997 assertNull(calibrator.getEstimatedCovariance());
6998 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6999 assertNull(calibrator.getGroundTruthGravityNorm());
7000 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7001 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7002
7003
7004 calibrator = null;
7005 try {
7006 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7007 true, new double[1], this);
7008 fail("IllegalArgumentException expected but not thrown");
7009 } catch (final IllegalArgumentException ignore) {
7010 }
7011 assertNull(calibrator);
7012 }
7013
7014 @Test
7015 public void testConstructor63() throws WrongSizeException {
7016 final Matrix ba = generateBa();
7017 final double biasX = ba.getElementAtIndex(0);
7018 final double biasY = ba.getElementAtIndex(1);
7019 final double biasZ = ba.getElementAtIndex(2);
7020
7021 KnownPositionAccelerometerCalibrator calibrator =
7022 new KnownPositionAccelerometerCalibrator(ba);
7023
7024
7025 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7026 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7027 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7028 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7029 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7030 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7031 final Acceleration bx2 = new Acceleration(0.0,
7032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7033 calibrator.getInitialBiasXAsAcceleration(bx2);
7034 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7035 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7036 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7037 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7038 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7039 final Acceleration by2 = new Acceleration(0.0,
7040 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7041 calibrator.getInitialBiasYAsAcceleration(by2);
7042 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7043 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7044 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7045 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7046 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7047 final Acceleration bz2 = new Acceleration(0.0,
7048 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7049 calibrator.getInitialBiasZAsAcceleration(bz2);
7050 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7051 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7052 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7053 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7054 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7055 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7056 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7057 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7058 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7059 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7060 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7061 final double[] bias1 = calibrator.getInitialBias();
7062 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7063 final double[] bias2 = new double[3];
7064 calibrator.getInitialBias(bias2);
7065 assertArrayEquals(bias1, bias2, 0.0);
7066 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7067 assertEquals(b1, ba);
7068 final Matrix b2 = new Matrix(3, 1);
7069 calibrator.getInitialBiasAsMatrix(b2);
7070 assertEquals(b1, b2);
7071 final Matrix ma1 = calibrator.getInitialMa();
7072 assertEquals(ma1, new Matrix(3, 3));
7073 final Matrix ma2 = new Matrix(3, 3);
7074 calibrator.getInitialMa(ma2);
7075 assertEquals(ma1, ma2);
7076 assertNull(calibrator.getEcefPosition());
7077 assertNull(calibrator.getNedPosition());
7078 assertFalse(calibrator.getNedPosition(null));
7079 assertNull(calibrator.getMeasurements());
7080 assertFalse(calibrator.isCommonAxisUsed());
7081 assertNull(calibrator.getListener());
7082 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7083 assertFalse(calibrator.isReady());
7084 assertFalse(calibrator.isRunning());
7085 assertNull(calibrator.getEstimatedBiases());
7086 assertFalse(calibrator.getEstimatedBiases(null));
7087 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7088 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7089 assertNull(calibrator.getEstimatedBiasFx());
7090 assertNull(calibrator.getEstimatedBiasFy());
7091 assertNull(calibrator.getEstimatedBiasFz());
7092 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7093 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7094 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7095 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7096 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7097 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7098 assertNull(calibrator.getEstimatedMa());
7099 assertNull(calibrator.getEstimatedSx());
7100 assertNull(calibrator.getEstimatedSy());
7101 assertNull(calibrator.getEstimatedSz());
7102 assertNull(calibrator.getEstimatedMxy());
7103 assertNull(calibrator.getEstimatedMxz());
7104 assertNull(calibrator.getEstimatedMyx());
7105 assertNull(calibrator.getEstimatedMyz());
7106 assertNull(calibrator.getEstimatedMzx());
7107 assertNull(calibrator.getEstimatedMzy());
7108 assertNull(calibrator.getEstimatedCovariance());
7109 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7110 assertNull(calibrator.getGroundTruthGravityNorm());
7111 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7112 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7113
7114
7115 calibrator = null;
7116 try {
7117 calibrator = new KnownPositionAccelerometerCalibrator(
7118 new Matrix(1, 1));
7119 fail("IllegalArgumentException expected but not thrown");
7120 } catch (final IllegalArgumentException ignore) {
7121 }
7122 try {
7123 calibrator = new KnownPositionAccelerometerCalibrator(
7124 new Matrix(1, 3));
7125 fail("IllegalArgumentException expected but not thrown");
7126 } catch (final IllegalArgumentException ignore) {
7127 }
7128 assertNull(calibrator);
7129 }
7130
7131 @Test
7132 public void testConstructor64() throws WrongSizeException {
7133 final Matrix ba = generateBa();
7134 final double biasX = ba.getElementAtIndex(0);
7135 final double biasY = ba.getElementAtIndex(1);
7136 final double biasZ = ba.getElementAtIndex(2);
7137
7138 KnownPositionAccelerometerCalibrator calibrator =
7139 new KnownPositionAccelerometerCalibrator(ba, this);
7140
7141
7142 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7143 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7144 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7145 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7146 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7147 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7148 final Acceleration bx2 = new Acceleration(0.0,
7149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7150 calibrator.getInitialBiasXAsAcceleration(bx2);
7151 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7152 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7153 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7154 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7155 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7156 final Acceleration by2 = new Acceleration(0.0,
7157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7158 calibrator.getInitialBiasYAsAcceleration(by2);
7159 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7160 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7161 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7162 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7163 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7164 final Acceleration bz2 = new Acceleration(0.0,
7165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7166 calibrator.getInitialBiasZAsAcceleration(bz2);
7167 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7168 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7169 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7170 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7171 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7172 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7173 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7174 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7175 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7176 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7177 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7178 final double[] bias1 = calibrator.getInitialBias();
7179 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7180 final double[] bias2 = new double[3];
7181 calibrator.getInitialBias(bias2);
7182 assertArrayEquals(bias1, bias2, 0.0);
7183 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7184 assertEquals(b1, ba);
7185 final Matrix b2 = new Matrix(3, 1);
7186 calibrator.getInitialBiasAsMatrix(b2);
7187 assertEquals(b1, b2);
7188 final Matrix ma1 = calibrator.getInitialMa();
7189 assertEquals(ma1, new Matrix(3, 3));
7190 final Matrix ma2 = new Matrix(3, 3);
7191 calibrator.getInitialMa(ma2);
7192 assertEquals(ma1, ma2);
7193 assertNull(calibrator.getEcefPosition());
7194 assertNull(calibrator.getNedPosition());
7195 assertFalse(calibrator.getNedPosition(null));
7196 assertNull(calibrator.getMeasurements());
7197 assertFalse(calibrator.isCommonAxisUsed());
7198 assertSame(calibrator.getListener(), this);
7199 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7200 assertFalse(calibrator.isReady());
7201 assertFalse(calibrator.isRunning());
7202 assertNull(calibrator.getEstimatedBiases());
7203 assertFalse(calibrator.getEstimatedBiases(null));
7204 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7205 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7206 assertNull(calibrator.getEstimatedBiasFx());
7207 assertNull(calibrator.getEstimatedBiasFy());
7208 assertNull(calibrator.getEstimatedBiasFz());
7209 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7210 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7211 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7212 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7213 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7214 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7215 assertNull(calibrator.getEstimatedMa());
7216 assertNull(calibrator.getEstimatedSx());
7217 assertNull(calibrator.getEstimatedSy());
7218 assertNull(calibrator.getEstimatedSz());
7219 assertNull(calibrator.getEstimatedMxy());
7220 assertNull(calibrator.getEstimatedMxz());
7221 assertNull(calibrator.getEstimatedMyx());
7222 assertNull(calibrator.getEstimatedMyz());
7223 assertNull(calibrator.getEstimatedMzx());
7224 assertNull(calibrator.getEstimatedMzy());
7225 assertNull(calibrator.getEstimatedCovariance());
7226 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7227 assertNull(calibrator.getGroundTruthGravityNorm());
7228 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7229 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7230
7231
7232 calibrator = null;
7233 try {
7234 calibrator = new KnownPositionAccelerometerCalibrator(
7235 new Matrix(1, 1));
7236 fail("IllegalArgumentException expected but not thrown");
7237 } catch (final IllegalArgumentException ignore) {
7238 }
7239 try {
7240 calibrator = new KnownPositionAccelerometerCalibrator(
7241 new Matrix(1, 3));
7242 fail("IllegalArgumentException expected but not thrown");
7243 } catch (final IllegalArgumentException ignore) {
7244 }
7245 assertNull(calibrator);
7246 }
7247
7248 @Test
7249 public void testConstructor65() throws WrongSizeException {
7250 final Collection<StandardDeviationBodyKinematics> measurements =
7251 Collections.emptyList();
7252
7253 final Matrix ba = generateBa();
7254 final double biasX = ba.getElementAtIndex(0);
7255 final double biasY = ba.getElementAtIndex(1);
7256 final double biasZ = ba.getElementAtIndex(2);
7257
7258 KnownPositionAccelerometerCalibrator calibrator =
7259 new KnownPositionAccelerometerCalibrator(measurements, ba);
7260
7261
7262 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7263 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7264 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7265 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7266 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7267 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7268 final Acceleration bx2 = new Acceleration(0.0,
7269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7270 calibrator.getInitialBiasXAsAcceleration(bx2);
7271 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7272 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7273 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7274 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7275 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7276 final Acceleration by2 = new Acceleration(0.0,
7277 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7278 calibrator.getInitialBiasYAsAcceleration(by2);
7279 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7280 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7281 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7282 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7283 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7284 final Acceleration bz2 = new Acceleration(0.0,
7285 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7286 calibrator.getInitialBiasZAsAcceleration(bz2);
7287 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7288 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7289 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7290 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7291 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7292 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7293 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7294 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7295 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7296 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7297 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7298 final double[] bias1 = calibrator.getInitialBias();
7299 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7300 final double[] bias2 = new double[3];
7301 calibrator.getInitialBias(bias2);
7302 assertArrayEquals(bias1, bias2, 0.0);
7303 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7304 assertEquals(b1, ba);
7305 final Matrix b2 = new Matrix(3, 1);
7306 calibrator.getInitialBiasAsMatrix(b2);
7307 assertEquals(b1, b2);
7308 final Matrix ma1 = calibrator.getInitialMa();
7309 assertEquals(ma1, new Matrix(3, 3));
7310 final Matrix ma2 = new Matrix(3, 3);
7311 calibrator.getInitialMa(ma2);
7312 assertEquals(ma1, ma2);
7313 assertNull(calibrator.getEcefPosition());
7314 assertNull(calibrator.getNedPosition());
7315 assertFalse(calibrator.getNedPosition(null));
7316 assertSame(calibrator.getMeasurements(), measurements);
7317 assertFalse(calibrator.isCommonAxisUsed());
7318 assertNull(calibrator.getListener());
7319 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7320 assertFalse(calibrator.isReady());
7321 assertFalse(calibrator.isRunning());
7322 assertNull(calibrator.getEstimatedBiases());
7323 assertFalse(calibrator.getEstimatedBiases(null));
7324 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7325 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7326 assertNull(calibrator.getEstimatedBiasFx());
7327 assertNull(calibrator.getEstimatedBiasFy());
7328 assertNull(calibrator.getEstimatedBiasFz());
7329 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7330 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7331 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7332 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7333 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7334 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7335 assertNull(calibrator.getEstimatedMa());
7336 assertNull(calibrator.getEstimatedSx());
7337 assertNull(calibrator.getEstimatedSy());
7338 assertNull(calibrator.getEstimatedSz());
7339 assertNull(calibrator.getEstimatedMxy());
7340 assertNull(calibrator.getEstimatedMxz());
7341 assertNull(calibrator.getEstimatedMyx());
7342 assertNull(calibrator.getEstimatedMyz());
7343 assertNull(calibrator.getEstimatedMzx());
7344 assertNull(calibrator.getEstimatedMzy());
7345 assertNull(calibrator.getEstimatedCovariance());
7346 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7347 assertNull(calibrator.getGroundTruthGravityNorm());
7348 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7349 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7350
7351
7352 calibrator = null;
7353 try {
7354 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7355 new Matrix(1, 1));
7356 fail("IllegalArgumentException expected but not thrown");
7357 } catch (final IllegalArgumentException ignore) {
7358 }
7359 try {
7360 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7361 new Matrix(1, 3));
7362 fail("IllegalArgumentException expected but not thrown");
7363 } catch (final IllegalArgumentException ignore) {
7364 }
7365 assertNull(calibrator);
7366 }
7367
7368 @Test
7369 public void testConstructor66() throws WrongSizeException {
7370 final Collection<StandardDeviationBodyKinematics> measurements =
7371 Collections.emptyList();
7372
7373 final Matrix ba = generateBa();
7374 final double biasX = ba.getElementAtIndex(0);
7375 final double biasY = ba.getElementAtIndex(1);
7376 final double biasZ = ba.getElementAtIndex(2);
7377
7378 KnownPositionAccelerometerCalibrator calibrator =
7379 new KnownPositionAccelerometerCalibrator(measurements, ba,
7380 this);
7381
7382
7383 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7384 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7385 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7386 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7387 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7388 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7389 final Acceleration bx2 = new Acceleration(0.0,
7390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7391 calibrator.getInitialBiasXAsAcceleration(bx2);
7392 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7393 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7394 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7395 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7396 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7397 final Acceleration by2 = new Acceleration(0.0,
7398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7399 calibrator.getInitialBiasYAsAcceleration(by2);
7400 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7401 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7402 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7403 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7404 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7405 final Acceleration bz2 = new Acceleration(0.0,
7406 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7407 calibrator.getInitialBiasZAsAcceleration(bz2);
7408 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7409 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7410 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7411 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7412 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7413 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7414 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7415 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7416 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7417 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7418 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7419 final double[] bias1 = calibrator.getInitialBias();
7420 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7421 final double[] bias2 = new double[3];
7422 calibrator.getInitialBias(bias2);
7423 assertArrayEquals(bias1, bias2, 0.0);
7424 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7425 assertEquals(b1, ba);
7426 final Matrix b2 = new Matrix(3, 1);
7427 calibrator.getInitialBiasAsMatrix(b2);
7428 assertEquals(b1, b2);
7429 final Matrix ma1 = calibrator.getInitialMa();
7430 assertEquals(ma1, new Matrix(3, 3));
7431 final Matrix ma2 = new Matrix(3, 3);
7432 calibrator.getInitialMa(ma2);
7433 assertEquals(ma1, ma2);
7434 assertNull(calibrator.getEcefPosition());
7435 assertNull(calibrator.getNedPosition());
7436 assertFalse(calibrator.getNedPosition(null));
7437 assertSame(calibrator.getMeasurements(), measurements);
7438 assertFalse(calibrator.isCommonAxisUsed());
7439 assertSame(calibrator.getListener(), this);
7440 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7441 assertFalse(calibrator.isReady());
7442 assertFalse(calibrator.isRunning());
7443 assertNull(calibrator.getEstimatedBiases());
7444 assertFalse(calibrator.getEstimatedBiases(null));
7445 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7446 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7447 assertNull(calibrator.getEstimatedBiasFx());
7448 assertNull(calibrator.getEstimatedBiasFy());
7449 assertNull(calibrator.getEstimatedBiasFz());
7450 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7451 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7452 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7453 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7454 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7455 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7456 assertNull(calibrator.getEstimatedMa());
7457 assertNull(calibrator.getEstimatedSx());
7458 assertNull(calibrator.getEstimatedSy());
7459 assertNull(calibrator.getEstimatedSz());
7460 assertNull(calibrator.getEstimatedMxy());
7461 assertNull(calibrator.getEstimatedMxz());
7462 assertNull(calibrator.getEstimatedMyx());
7463 assertNull(calibrator.getEstimatedMyz());
7464 assertNull(calibrator.getEstimatedMzx());
7465 assertNull(calibrator.getEstimatedMzy());
7466 assertNull(calibrator.getEstimatedCovariance());
7467 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7468 assertNull(calibrator.getGroundTruthGravityNorm());
7469 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7470 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7471
7472
7473 calibrator = null;
7474 try {
7475 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7476 new Matrix(1, 1), this);
7477 fail("IllegalArgumentException expected but not thrown");
7478 } catch (final IllegalArgumentException ignore) {
7479 }
7480 try {
7481 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7482 new Matrix(1, 3), this);
7483 fail("IllegalArgumentException expected but not thrown");
7484 } catch (final IllegalArgumentException ignore) {
7485 }
7486 assertNull(calibrator);
7487 }
7488
7489 @Test
7490 public void testConstructor67() throws WrongSizeException {
7491 final Matrix ba = generateBa();
7492 final double biasX = ba.getElementAtIndex(0);
7493 final double biasY = ba.getElementAtIndex(1);
7494 final double biasZ = ba.getElementAtIndex(2);
7495
7496 KnownPositionAccelerometerCalibrator calibrator =
7497 new KnownPositionAccelerometerCalibrator(true, ba);
7498
7499
7500 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7501 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7502 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7503 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7504 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7505 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7506 final Acceleration bx2 = new Acceleration(0.0,
7507 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7508 calibrator.getInitialBiasXAsAcceleration(bx2);
7509 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7510 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7511 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7512 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7513 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7514 final Acceleration by2 = new Acceleration(0.0,
7515 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7516 calibrator.getInitialBiasYAsAcceleration(by2);
7517 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7518 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7519 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7520 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7521 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7522 final Acceleration bz2 = new Acceleration(0.0,
7523 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7524 calibrator.getInitialBiasZAsAcceleration(bz2);
7525 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7526 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7527 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7528 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7529 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7530 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7531 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7532 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7533 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7534 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7535 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7536 final double[] bias1 = calibrator.getInitialBias();
7537 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7538 final double[] bias2 = new double[3];
7539 calibrator.getInitialBias(bias2);
7540 assertArrayEquals(bias1, bias2, 0.0);
7541 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7542 assertEquals(b1, ba);
7543 final Matrix b2 = new Matrix(3, 1);
7544 calibrator.getInitialBiasAsMatrix(b2);
7545 assertEquals(b1, b2);
7546 final Matrix ma1 = calibrator.getInitialMa();
7547 assertEquals(ma1, new Matrix(3, 3));
7548 final Matrix ma2 = new Matrix(3, 3);
7549 calibrator.getInitialMa(ma2);
7550 assertEquals(ma1, ma2);
7551 assertNull(calibrator.getEcefPosition());
7552 assertNull(calibrator.getNedPosition());
7553 assertFalse(calibrator.getNedPosition(null));
7554 assertNull(calibrator.getMeasurements());
7555 assertTrue(calibrator.isCommonAxisUsed());
7556 assertNull(calibrator.getListener());
7557 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7558 assertFalse(calibrator.isReady());
7559 assertFalse(calibrator.isRunning());
7560 assertNull(calibrator.getEstimatedBiases());
7561 assertFalse(calibrator.getEstimatedBiases(null));
7562 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7563 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7564 assertNull(calibrator.getEstimatedBiasFx());
7565 assertNull(calibrator.getEstimatedBiasFy());
7566 assertNull(calibrator.getEstimatedBiasFz());
7567 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7568 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7569 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7570 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7571 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7572 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7573 assertNull(calibrator.getEstimatedMa());
7574 assertNull(calibrator.getEstimatedSx());
7575 assertNull(calibrator.getEstimatedSy());
7576 assertNull(calibrator.getEstimatedSz());
7577 assertNull(calibrator.getEstimatedMxy());
7578 assertNull(calibrator.getEstimatedMxz());
7579 assertNull(calibrator.getEstimatedMyx());
7580 assertNull(calibrator.getEstimatedMyz());
7581 assertNull(calibrator.getEstimatedMzx());
7582 assertNull(calibrator.getEstimatedMzy());
7583 assertNull(calibrator.getEstimatedCovariance());
7584 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7585 assertNull(calibrator.getGroundTruthGravityNorm());
7586 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7587 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7588
7589
7590 calibrator = null;
7591 try {
7592 calibrator = new KnownPositionAccelerometerCalibrator(
7593 true, new Matrix(1, 1));
7594 fail("IllegalArgumentException expected but not thrown");
7595 } catch (final IllegalArgumentException ignore) {
7596 }
7597 try {
7598 calibrator = new KnownPositionAccelerometerCalibrator(
7599 true, new Matrix(1, 3));
7600 fail("IllegalArgumentException expected but not thrown");
7601 } catch (final IllegalArgumentException ignore) {
7602 }
7603 assertNull(calibrator);
7604 }
7605
7606 @Test
7607 public void testConstructor68() throws WrongSizeException {
7608 final Matrix ba = generateBa();
7609 final double biasX = ba.getElementAtIndex(0);
7610 final double biasY = ba.getElementAtIndex(1);
7611 final double biasZ = ba.getElementAtIndex(2);
7612
7613 KnownPositionAccelerometerCalibrator calibrator =
7614 new KnownPositionAccelerometerCalibrator(true, ba,
7615 this);
7616
7617
7618 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7619 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7620 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7621 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7622 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7623 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7624 final Acceleration bx2 = new Acceleration(0.0,
7625 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7626 calibrator.getInitialBiasXAsAcceleration(bx2);
7627 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7628 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7629 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7630 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7631 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7632 final Acceleration by2 = new Acceleration(0.0,
7633 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7634 calibrator.getInitialBiasYAsAcceleration(by2);
7635 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7636 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7637 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7638 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7639 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7640 final Acceleration bz2 = new Acceleration(0.0,
7641 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7642 calibrator.getInitialBiasZAsAcceleration(bz2);
7643 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7644 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7645 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7646 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7647 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7648 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7649 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7650 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7651 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7652 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7653 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7654 final double[] bias1 = calibrator.getInitialBias();
7655 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7656 final double[] bias2 = new double[3];
7657 calibrator.getInitialBias(bias2);
7658 assertArrayEquals(bias1, bias2, 0.0);
7659 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7660 assertEquals(b1, ba);
7661 final Matrix b2 = new Matrix(3, 1);
7662 calibrator.getInitialBiasAsMatrix(b2);
7663 assertEquals(b1, b2);
7664 final Matrix ma1 = calibrator.getInitialMa();
7665 assertEquals(ma1, new Matrix(3, 3));
7666 final Matrix ma2 = new Matrix(3, 3);
7667 calibrator.getInitialMa(ma2);
7668 assertEquals(ma1, ma2);
7669 assertNull(calibrator.getEcefPosition());
7670 assertNull(calibrator.getNedPosition());
7671 assertFalse(calibrator.getNedPosition(null));
7672 assertNull(calibrator.getMeasurements());
7673 assertTrue(calibrator.isCommonAxisUsed());
7674 assertSame(calibrator.getListener(), this);
7675 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7676 assertFalse(calibrator.isReady());
7677 assertFalse(calibrator.isRunning());
7678 assertNull(calibrator.getEstimatedBiases());
7679 assertFalse(calibrator.getEstimatedBiases(null));
7680 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7681 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7682 assertNull(calibrator.getEstimatedBiasFx());
7683 assertNull(calibrator.getEstimatedBiasFy());
7684 assertNull(calibrator.getEstimatedBiasFz());
7685 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7686 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7687 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7688 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7689 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7690 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7691 assertNull(calibrator.getEstimatedMa());
7692 assertNull(calibrator.getEstimatedSx());
7693 assertNull(calibrator.getEstimatedSy());
7694 assertNull(calibrator.getEstimatedSz());
7695 assertNull(calibrator.getEstimatedMxy());
7696 assertNull(calibrator.getEstimatedMxz());
7697 assertNull(calibrator.getEstimatedMyx());
7698 assertNull(calibrator.getEstimatedMyz());
7699 assertNull(calibrator.getEstimatedMzx());
7700 assertNull(calibrator.getEstimatedMzy());
7701 assertNull(calibrator.getEstimatedCovariance());
7702 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7703 assertNull(calibrator.getGroundTruthGravityNorm());
7704 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7705 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7706
7707
7708 calibrator = null;
7709 try {
7710 calibrator = new KnownPositionAccelerometerCalibrator(
7711 true, new Matrix(1, 1),
7712 this);
7713 fail("IllegalArgumentException expected but not thrown");
7714 } catch (final IllegalArgumentException ignore) {
7715 }
7716 try {
7717 calibrator = new KnownPositionAccelerometerCalibrator(
7718 true, new Matrix(1, 3),
7719 this);
7720 fail("IllegalArgumentException expected but not thrown");
7721 } catch (final IllegalArgumentException ignore) {
7722 }
7723 assertNull(calibrator);
7724 }
7725
7726 @Test
7727 public void testConstructor69() throws WrongSizeException {
7728 final Collection<StandardDeviationBodyKinematics> measurements =
7729 Collections.emptyList();
7730
7731 final Matrix ba = generateBa();
7732 final double biasX = ba.getElementAtIndex(0);
7733 final double biasY = ba.getElementAtIndex(1);
7734 final double biasZ = ba.getElementAtIndex(2);
7735
7736 KnownPositionAccelerometerCalibrator calibrator =
7737 new KnownPositionAccelerometerCalibrator(measurements,
7738 true, ba);
7739
7740
7741 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7742 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7743 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7744 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7745 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7746 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7747 final Acceleration bx2 = new Acceleration(0.0,
7748 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7749 calibrator.getInitialBiasXAsAcceleration(bx2);
7750 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7751 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7752 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7753 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7754 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7755 final Acceleration by2 = new Acceleration(0.0,
7756 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7757 calibrator.getInitialBiasYAsAcceleration(by2);
7758 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7759 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7760 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7761 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7762 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7763 final Acceleration bz2 = new Acceleration(0.0,
7764 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7765 calibrator.getInitialBiasZAsAcceleration(bz2);
7766 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7767 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7768 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7769 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7770 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7771 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7772 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7773 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7774 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7775 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7776 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7777 final double[] bias1 = calibrator.getInitialBias();
7778 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7779 final double[] bias2 = new double[3];
7780 calibrator.getInitialBias(bias2);
7781 assertArrayEquals(bias1, bias2, 0.0);
7782 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7783 assertEquals(b1, ba);
7784 final Matrix b2 = new Matrix(3, 1);
7785 calibrator.getInitialBiasAsMatrix(b2);
7786 assertEquals(b1, b2);
7787 final Matrix ma1 = calibrator.getInitialMa();
7788 assertEquals(ma1, new Matrix(3, 3));
7789 final Matrix ma2 = new Matrix(3, 3);
7790 calibrator.getInitialMa(ma2);
7791 assertEquals(ma1, ma2);
7792 assertNull(calibrator.getEcefPosition());
7793 assertNull(calibrator.getNedPosition());
7794 assertFalse(calibrator.getNedPosition(null));
7795 assertSame(calibrator.getMeasurements(), measurements);
7796 assertTrue(calibrator.isCommonAxisUsed());
7797 assertNull(calibrator.getListener());
7798 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7799 assertFalse(calibrator.isReady());
7800 assertFalse(calibrator.isRunning());
7801 assertNull(calibrator.getEstimatedBiases());
7802 assertFalse(calibrator.getEstimatedBiases(null));
7803 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7804 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7805 assertNull(calibrator.getEstimatedBiasFx());
7806 assertNull(calibrator.getEstimatedBiasFy());
7807 assertNull(calibrator.getEstimatedBiasFz());
7808 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7809 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7810 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7811 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7812 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7813 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7814 assertNull(calibrator.getEstimatedMa());
7815 assertNull(calibrator.getEstimatedSx());
7816 assertNull(calibrator.getEstimatedSy());
7817 assertNull(calibrator.getEstimatedSz());
7818 assertNull(calibrator.getEstimatedMxy());
7819 assertNull(calibrator.getEstimatedMxz());
7820 assertNull(calibrator.getEstimatedMyx());
7821 assertNull(calibrator.getEstimatedMyz());
7822 assertNull(calibrator.getEstimatedMzx());
7823 assertNull(calibrator.getEstimatedMzy());
7824 assertNull(calibrator.getEstimatedCovariance());
7825 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7826 assertNull(calibrator.getGroundTruthGravityNorm());
7827 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7828 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7829
7830
7831 calibrator = null;
7832 try {
7833 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7834 true, new Matrix(1, 1));
7835 fail("IllegalArgumentException expected but not thrown");
7836 } catch (final IllegalArgumentException ignore) {
7837 }
7838 try {
7839 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7840 true, new Matrix(1, 3));
7841 fail("IllegalArgumentException expected but not thrown");
7842 } catch (final IllegalArgumentException ignore) {
7843 }
7844 assertNull(calibrator);
7845 }
7846
7847 @Test
7848 public void testConstructor70() throws WrongSizeException {
7849 final Collection<StandardDeviationBodyKinematics> measurements =
7850 Collections.emptyList();
7851
7852 final Matrix ba = generateBa();
7853 final double biasX = ba.getElementAtIndex(0);
7854 final double biasY = ba.getElementAtIndex(1);
7855 final double biasZ = ba.getElementAtIndex(2);
7856
7857 KnownPositionAccelerometerCalibrator calibrator =
7858 new KnownPositionAccelerometerCalibrator(measurements,
7859 true, ba, this);
7860
7861
7862 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7863 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7864 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7865 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7866 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7867 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7868 final Acceleration bx2 = new Acceleration(0.0,
7869 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7870 calibrator.getInitialBiasXAsAcceleration(bx2);
7871 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7872 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7873 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7874 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7875 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7876 final Acceleration by2 = new Acceleration(0.0,
7877 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7878 calibrator.getInitialBiasYAsAcceleration(by2);
7879 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7880 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7881 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7882 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7883 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7884 final Acceleration bz2 = new Acceleration(0.0,
7885 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7886 calibrator.getInitialBiasZAsAcceleration(bz2);
7887 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7888 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7889 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7890 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7891 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7892 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7893 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7894 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7895 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7896 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7897 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7898 final double[] bias1 = calibrator.getInitialBias();
7899 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7900 final double[] bias2 = new double[3];
7901 calibrator.getInitialBias(bias2);
7902 assertArrayEquals(bias1, bias2, 0.0);
7903 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7904 assertEquals(b1, ba);
7905 final Matrix b2 = new Matrix(3, 1);
7906 calibrator.getInitialBiasAsMatrix(b2);
7907 assertEquals(b1, b2);
7908 final Matrix ma1 = calibrator.getInitialMa();
7909 assertEquals(ma1, new Matrix(3, 3));
7910 final Matrix ma2 = new Matrix(3, 3);
7911 calibrator.getInitialMa(ma2);
7912 assertEquals(ma1, ma2);
7913 assertNull(calibrator.getEcefPosition());
7914 assertNull(calibrator.getNedPosition());
7915 assertFalse(calibrator.getNedPosition(null));
7916 assertSame(calibrator.getMeasurements(), measurements);
7917 assertTrue(calibrator.isCommonAxisUsed());
7918 assertSame(calibrator.getListener(), this);
7919 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7920 assertFalse(calibrator.isReady());
7921 assertFalse(calibrator.isRunning());
7922 assertNull(calibrator.getEstimatedBiases());
7923 assertFalse(calibrator.getEstimatedBiases(null));
7924 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7925 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7926 assertNull(calibrator.getEstimatedBiasFx());
7927 assertNull(calibrator.getEstimatedBiasFy());
7928 assertNull(calibrator.getEstimatedBiasFz());
7929 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7930 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7931 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7932 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7933 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7934 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7935 assertNull(calibrator.getEstimatedMa());
7936 assertNull(calibrator.getEstimatedSx());
7937 assertNull(calibrator.getEstimatedSy());
7938 assertNull(calibrator.getEstimatedSz());
7939 assertNull(calibrator.getEstimatedMxy());
7940 assertNull(calibrator.getEstimatedMxz());
7941 assertNull(calibrator.getEstimatedMyx());
7942 assertNull(calibrator.getEstimatedMyz());
7943 assertNull(calibrator.getEstimatedMzx());
7944 assertNull(calibrator.getEstimatedMzy());
7945 assertNull(calibrator.getEstimatedCovariance());
7946 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7947 assertNull(calibrator.getGroundTruthGravityNorm());
7948 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7949 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7950
7951
7952 calibrator = null;
7953 try {
7954 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7955 true, new Matrix(1, 1),
7956 this);
7957 fail("IllegalArgumentException expected but not thrown");
7958 } catch (final IllegalArgumentException ignore) {
7959 }
7960 try {
7961 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7962 true, new Matrix(1, 3),
7963 this);
7964 fail("IllegalArgumentException expected but not thrown");
7965 } catch (final IllegalArgumentException ignore) {
7966 }
7967 assertNull(calibrator);
7968 }
7969
7970 @Test
7971 public void testConstructor71() throws WrongSizeException {
7972 final Matrix ba = generateBa();
7973 final double biasX = ba.getElementAtIndex(0);
7974 final double biasY = ba.getElementAtIndex(1);
7975 final double biasZ = ba.getElementAtIndex(2);
7976
7977 final Matrix ma = generateMaCommonAxis();
7978 final double sx = ma.getElementAt(0, 0);
7979 final double sy = ma.getElementAt(1, 1);
7980 final double sz = ma.getElementAt(2, 2);
7981 final double mxy = ma.getElementAt(0, 1);
7982 final double mxz = ma.getElementAt(0, 2);
7983 final double myx = ma.getElementAt(1, 0);
7984 final double myz = ma.getElementAt(1, 2);
7985 final double mzx = ma.getElementAt(2, 0);
7986 final double mzy = ma.getElementAt(2, 1);
7987
7988 KnownPositionAccelerometerCalibrator calibrator =
7989 new KnownPositionAccelerometerCalibrator(ba, ma);
7990
7991
7992 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7993 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7994 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7995 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7996 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7997 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7998 final Acceleration bx2 = new Acceleration(0.0,
7999 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8000 calibrator.getInitialBiasXAsAcceleration(bx2);
8001 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8002 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8003 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8004 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8005 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8006 final Acceleration by2 = new Acceleration(0.0,
8007 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8008 calibrator.getInitialBiasYAsAcceleration(by2);
8009 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8010 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8011 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8012 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8013 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8014 final Acceleration bz2 = new Acceleration(0.0,
8015 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8016 calibrator.getInitialBiasZAsAcceleration(bz2);
8017 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8018 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8019 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8020 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8021 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8022 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8023 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8024 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8025 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8026 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8027 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8028 final double[] bias1 = calibrator.getInitialBias();
8029 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8030 final double[] bias2 = new double[3];
8031 calibrator.getInitialBias(bias2);
8032 assertArrayEquals(bias1, bias2, 0.0);
8033 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8034 assertEquals(b1, ba);
8035 final Matrix b2 = new Matrix(3, 1);
8036 calibrator.getInitialBiasAsMatrix(b2);
8037 assertEquals(b1, b2);
8038 final Matrix ma1 = new Matrix(3, 3);
8039 ma1.setSubmatrix(0, 0,
8040 2, 2,
8041 new double[]{sx, myx, mzx,
8042 mxy, sy, mzy,
8043 mxz, myz, sz});
8044 assertEquals(calibrator.getInitialMa(), ma1);
8045 final Matrix ma2 = new Matrix(3, 3);
8046 calibrator.getInitialMa(ma2);
8047 assertEquals(ma1, ma2);
8048 assertNull(calibrator.getEcefPosition());
8049 assertNull(calibrator.getNedPosition());
8050 assertFalse(calibrator.getNedPosition(null));
8051 assertNull(calibrator.getMeasurements());
8052 assertFalse(calibrator.isCommonAxisUsed());
8053 assertNull(calibrator.getListener());
8054 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8055 assertFalse(calibrator.isReady());
8056 assertFalse(calibrator.isRunning());
8057 assertNull(calibrator.getEstimatedBiases());
8058 assertFalse(calibrator.getEstimatedBiases(null));
8059 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8060 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8061 assertNull(calibrator.getEstimatedBiasFx());
8062 assertNull(calibrator.getEstimatedBiasFy());
8063 assertNull(calibrator.getEstimatedBiasFz());
8064 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8065 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8066 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8067 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8068 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8069 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8070 assertNull(calibrator.getEstimatedMa());
8071 assertNull(calibrator.getEstimatedSx());
8072 assertNull(calibrator.getEstimatedSy());
8073 assertNull(calibrator.getEstimatedSz());
8074 assertNull(calibrator.getEstimatedMxy());
8075 assertNull(calibrator.getEstimatedMxz());
8076 assertNull(calibrator.getEstimatedMyx());
8077 assertNull(calibrator.getEstimatedMyz());
8078 assertNull(calibrator.getEstimatedMzx());
8079 assertNull(calibrator.getEstimatedMzy());
8080 assertNull(calibrator.getEstimatedCovariance());
8081 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8082 assertNull(calibrator.getGroundTruthGravityNorm());
8083 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8084 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8085
8086
8087 calibrator = null;
8088 try {
8089 calibrator = new KnownPositionAccelerometerCalibrator(
8090 new Matrix(1, 1), ma);
8091 fail("IllegalArgumentException expected but not thrown");
8092 } catch (final IllegalArgumentException ignore) {
8093 }
8094 try {
8095 calibrator = new KnownPositionAccelerometerCalibrator(
8096 new Matrix(1, 3), ma);
8097 fail("IllegalArgumentException expected but not thrown");
8098 } catch (final IllegalArgumentException ignore) {
8099 }
8100 try {
8101 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8102 new Matrix(1, 3));
8103 fail("IllegalArgumentException expected but not thrown");
8104 } catch (final IllegalArgumentException ignore) {
8105 }
8106 try {
8107 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8108 new Matrix(3, 1));
8109 fail("IllegalArgumentException expected but not thrown");
8110 } catch (final IllegalArgumentException ignore) {
8111 }
8112 assertNull(calibrator);
8113 }
8114
8115 @Test
8116 public void testConstructor72() throws WrongSizeException {
8117 final Matrix ba = generateBa();
8118 final double biasX = ba.getElementAtIndex(0);
8119 final double biasY = ba.getElementAtIndex(1);
8120 final double biasZ = ba.getElementAtIndex(2);
8121
8122 final Matrix ma = generateMaCommonAxis();
8123 final double sx = ma.getElementAt(0, 0);
8124 final double sy = ma.getElementAt(1, 1);
8125 final double sz = ma.getElementAt(2, 2);
8126 final double mxy = ma.getElementAt(0, 1);
8127 final double mxz = ma.getElementAt(0, 2);
8128 final double myx = ma.getElementAt(1, 0);
8129 final double myz = ma.getElementAt(1, 2);
8130 final double mzx = ma.getElementAt(2, 0);
8131 final double mzy = ma.getElementAt(2, 1);
8132
8133 KnownPositionAccelerometerCalibrator calibrator =
8134 new KnownPositionAccelerometerCalibrator(ba, ma, this);
8135
8136
8137 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8138 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8139 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8140 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8141 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8142 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8143 final Acceleration bx2 = new Acceleration(0.0,
8144 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8145 calibrator.getInitialBiasXAsAcceleration(bx2);
8146 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8147 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8148 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8149 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8150 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8151 final Acceleration by2 = new Acceleration(0.0,
8152 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8153 calibrator.getInitialBiasYAsAcceleration(by2);
8154 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8155 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8156 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8157 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8158 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8159 final Acceleration bz2 = new Acceleration(0.0,
8160 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8161 calibrator.getInitialBiasZAsAcceleration(bz2);
8162 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8163 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8164 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8165 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8166 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8167 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8168 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8169 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8170 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8171 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8172 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8173 final double[] bias1 = calibrator.getInitialBias();
8174 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8175 final double[] bias2 = new double[3];
8176 calibrator.getInitialBias(bias2);
8177 assertArrayEquals(bias1, bias2, 0.0);
8178 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8179 assertEquals(b1, ba);
8180 final Matrix b2 = new Matrix(3, 1);
8181 calibrator.getInitialBiasAsMatrix(b2);
8182 assertEquals(b1, b2);
8183 final Matrix ma1 = new Matrix(3, 3);
8184 ma1.setSubmatrix(0, 0,
8185 2, 2,
8186 new double[]{sx, myx, mzx,
8187 mxy, sy, mzy,
8188 mxz, myz, sz});
8189 assertEquals(calibrator.getInitialMa(), ma1);
8190 final Matrix ma2 = new Matrix(3, 3);
8191 calibrator.getInitialMa(ma2);
8192 assertEquals(ma1, ma2);
8193 assertNull(calibrator.getEcefPosition());
8194 assertNull(calibrator.getNedPosition());
8195 assertFalse(calibrator.getNedPosition(null));
8196 assertNull(calibrator.getMeasurements());
8197 assertFalse(calibrator.isCommonAxisUsed());
8198 assertSame(calibrator.getListener(), this);
8199 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8200 assertFalse(calibrator.isReady());
8201 assertFalse(calibrator.isRunning());
8202 assertNull(calibrator.getEstimatedBiases());
8203 assertFalse(calibrator.getEstimatedBiases(null));
8204 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8205 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8206 assertNull(calibrator.getEstimatedBiasFx());
8207 assertNull(calibrator.getEstimatedBiasFy());
8208 assertNull(calibrator.getEstimatedBiasFz());
8209 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8210 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8211 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8212 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8213 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8214 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8215 assertNull(calibrator.getEstimatedMa());
8216 assertNull(calibrator.getEstimatedSx());
8217 assertNull(calibrator.getEstimatedSy());
8218 assertNull(calibrator.getEstimatedSz());
8219 assertNull(calibrator.getEstimatedMxy());
8220 assertNull(calibrator.getEstimatedMxz());
8221 assertNull(calibrator.getEstimatedMyx());
8222 assertNull(calibrator.getEstimatedMyz());
8223 assertNull(calibrator.getEstimatedMzx());
8224 assertNull(calibrator.getEstimatedMzy());
8225 assertNull(calibrator.getEstimatedCovariance());
8226 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8227 assertNull(calibrator.getGroundTruthGravityNorm());
8228 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8229 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8230
8231
8232 calibrator = null;
8233 try {
8234 calibrator = new KnownPositionAccelerometerCalibrator(
8235 new Matrix(1, 1), ma, this);
8236 fail("IllegalArgumentException expected but not thrown");
8237 } catch (final IllegalArgumentException ignore) {
8238 }
8239 try {
8240 calibrator = new KnownPositionAccelerometerCalibrator(
8241 new Matrix(1, 3), ma, this);
8242 fail("IllegalArgumentException expected but not thrown");
8243 } catch (final IllegalArgumentException ignore) {
8244 }
8245 try {
8246 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8247 new Matrix(1, 3), this);
8248 fail("IllegalArgumentException expected but not thrown");
8249 } catch (final IllegalArgumentException ignore) {
8250 }
8251 try {
8252 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8253 new Matrix(3, 1), this);
8254 fail("IllegalArgumentException expected but not thrown");
8255 } catch (final IllegalArgumentException ignore) {
8256 }
8257 assertNull(calibrator);
8258 }
8259
8260 @Test
8261 public void testConstructor73() throws WrongSizeException {
8262 final Collection<StandardDeviationBodyKinematics> measurements =
8263 Collections.emptyList();
8264
8265 final Matrix ba = generateBa();
8266 final double biasX = ba.getElementAtIndex(0);
8267 final double biasY = ba.getElementAtIndex(1);
8268 final double biasZ = ba.getElementAtIndex(2);
8269
8270 final Matrix ma = generateMaCommonAxis();
8271 final double sx = ma.getElementAt(0, 0);
8272 final double sy = ma.getElementAt(1, 1);
8273 final double sz = ma.getElementAt(2, 2);
8274 final double mxy = ma.getElementAt(0, 1);
8275 final double mxz = ma.getElementAt(0, 2);
8276 final double myx = ma.getElementAt(1, 0);
8277 final double myz = ma.getElementAt(1, 2);
8278 final double mzx = ma.getElementAt(2, 0);
8279 final double mzy = ma.getElementAt(2, 1);
8280
8281 KnownPositionAccelerometerCalibrator calibrator =
8282 new KnownPositionAccelerometerCalibrator(measurements, ba, ma);
8283
8284
8285 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8286 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8287 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8288 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8289 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8290 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8291 final Acceleration bx2 = new Acceleration(0.0,
8292 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8293 calibrator.getInitialBiasXAsAcceleration(bx2);
8294 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8295 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8296 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8297 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8298 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8299 final Acceleration by2 = new Acceleration(0.0,
8300 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8301 calibrator.getInitialBiasYAsAcceleration(by2);
8302 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8303 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8304 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8305 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8306 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8307 final Acceleration bz2 = new Acceleration(0.0,
8308 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8309 calibrator.getInitialBiasZAsAcceleration(bz2);
8310 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8311 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8312 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8313 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8314 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8315 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8316 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8317 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8318 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8319 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8320 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8321 final double[] bias1 = calibrator.getInitialBias();
8322 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8323 final double[] bias2 = new double[3];
8324 calibrator.getInitialBias(bias2);
8325 assertArrayEquals(bias1, bias2, 0.0);
8326 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8327 assertEquals(b1, ba);
8328 final Matrix b2 = new Matrix(3, 1);
8329 calibrator.getInitialBiasAsMatrix(b2);
8330 assertEquals(b1, b2);
8331 final Matrix ma1 = new Matrix(3, 3);
8332 ma1.setSubmatrix(0, 0,
8333 2, 2,
8334 new double[]{sx, myx, mzx,
8335 mxy, sy, mzy,
8336 mxz, myz, sz});
8337 assertEquals(calibrator.getInitialMa(), ma1);
8338 final Matrix ma2 = new Matrix(3, 3);
8339 calibrator.getInitialMa(ma2);
8340 assertEquals(ma1, ma2);
8341 assertNull(calibrator.getEcefPosition());
8342 assertNull(calibrator.getNedPosition());
8343 assertFalse(calibrator.getNedPosition(null));
8344 assertSame(calibrator.getMeasurements(), measurements);
8345 assertFalse(calibrator.isCommonAxisUsed());
8346 assertNull(calibrator.getListener());
8347 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8348 assertFalse(calibrator.isReady());
8349 assertFalse(calibrator.isRunning());
8350 assertNull(calibrator.getEstimatedBiases());
8351 assertFalse(calibrator.getEstimatedBiases(null));
8352 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8353 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8354 assertNull(calibrator.getEstimatedBiasFx());
8355 assertNull(calibrator.getEstimatedBiasFy());
8356 assertNull(calibrator.getEstimatedBiasFz());
8357 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8358 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8359 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8360 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8361 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8362 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8363 assertNull(calibrator.getEstimatedMa());
8364 assertNull(calibrator.getEstimatedSx());
8365 assertNull(calibrator.getEstimatedSy());
8366 assertNull(calibrator.getEstimatedSz());
8367 assertNull(calibrator.getEstimatedMxy());
8368 assertNull(calibrator.getEstimatedMxz());
8369 assertNull(calibrator.getEstimatedMyx());
8370 assertNull(calibrator.getEstimatedMyz());
8371 assertNull(calibrator.getEstimatedMzx());
8372 assertNull(calibrator.getEstimatedMzy());
8373 assertNull(calibrator.getEstimatedCovariance());
8374 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8375 assertNull(calibrator.getGroundTruthGravityNorm());
8376 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8377 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8378
8379
8380 calibrator = null;
8381 try {
8382 calibrator = new KnownPositionAccelerometerCalibrator(
8383 measurements, new Matrix(1, 1), ma);
8384 fail("IllegalArgumentException expected but not thrown");
8385 } catch (final IllegalArgumentException ignore) {
8386 }
8387 try {
8388 calibrator = new KnownPositionAccelerometerCalibrator(
8389 measurements, new Matrix(1, 3), ma);
8390 fail("IllegalArgumentException expected but not thrown");
8391 } catch (final IllegalArgumentException ignore) {
8392 }
8393 try {
8394 calibrator = new KnownPositionAccelerometerCalibrator(
8395 measurements, ba, new Matrix(1, 3));
8396 fail("IllegalArgumentException expected but not thrown");
8397 } catch (final IllegalArgumentException ignore) {
8398 }
8399 try {
8400 calibrator = new KnownPositionAccelerometerCalibrator(
8401 measurements, ba, new Matrix(3, 1));
8402 fail("IllegalArgumentException expected but not thrown");
8403 } catch (final IllegalArgumentException ignore) {
8404 }
8405 assertNull(calibrator);
8406 }
8407
8408 @Test
8409 public void testConstructor74() throws WrongSizeException {
8410 final Collection<StandardDeviationBodyKinematics> measurements =
8411 Collections.emptyList();
8412
8413 final Matrix ba = generateBa();
8414 final double biasX = ba.getElementAtIndex(0);
8415 final double biasY = ba.getElementAtIndex(1);
8416 final double biasZ = ba.getElementAtIndex(2);
8417
8418 final Matrix ma = generateMaCommonAxis();
8419 final double sx = ma.getElementAt(0, 0);
8420 final double sy = ma.getElementAt(1, 1);
8421 final double sz = ma.getElementAt(2, 2);
8422 final double mxy = ma.getElementAt(0, 1);
8423 final double mxz = ma.getElementAt(0, 2);
8424 final double myx = ma.getElementAt(1, 0);
8425 final double myz = ma.getElementAt(1, 2);
8426 final double mzx = ma.getElementAt(2, 0);
8427 final double mzy = ma.getElementAt(2, 1);
8428
8429 KnownPositionAccelerometerCalibrator calibrator =
8430 new KnownPositionAccelerometerCalibrator(measurements, ba, ma,
8431 this);
8432
8433
8434 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8435 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8436 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8437 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8438 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8439 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8440 final Acceleration bx2 = new Acceleration(0.0,
8441 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8442 calibrator.getInitialBiasXAsAcceleration(bx2);
8443 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8444 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8445 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8446 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8447 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8448 final Acceleration by2 = new Acceleration(0.0,
8449 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8450 calibrator.getInitialBiasYAsAcceleration(by2);
8451 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8452 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8453 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8454 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8455 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8456 final Acceleration bz2 = new Acceleration(0.0,
8457 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8458 calibrator.getInitialBiasZAsAcceleration(bz2);
8459 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8460 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8461 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8462 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8463 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8464 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8465 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8466 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8467 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8468 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8469 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8470 final double[] bias1 = calibrator.getInitialBias();
8471 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8472 final double[] bias2 = new double[3];
8473 calibrator.getInitialBias(bias2);
8474 assertArrayEquals(bias1, bias2, 0.0);
8475 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8476 assertEquals(b1, ba);
8477 final Matrix b2 = new Matrix(3, 1);
8478 calibrator.getInitialBiasAsMatrix(b2);
8479 assertEquals(b1, b2);
8480 final Matrix ma1 = new Matrix(3, 3);
8481 ma1.setSubmatrix(0, 0,
8482 2, 2,
8483 new double[]{sx, myx, mzx,
8484 mxy, sy, mzy,
8485 mxz, myz, sz});
8486 assertEquals(calibrator.getInitialMa(), ma1);
8487 final Matrix ma2 = new Matrix(3, 3);
8488 calibrator.getInitialMa(ma2);
8489 assertEquals(ma1, ma2);
8490 assertNull(calibrator.getEcefPosition());
8491 assertNull(calibrator.getNedPosition());
8492 assertFalse(calibrator.getNedPosition(null));
8493 assertSame(calibrator.getMeasurements(), measurements);
8494 assertFalse(calibrator.isCommonAxisUsed());
8495 assertSame(calibrator.getListener(), this);
8496 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8497 assertFalse(calibrator.isReady());
8498 assertFalse(calibrator.isRunning());
8499 assertNull(calibrator.getEstimatedBiases());
8500 assertFalse(calibrator.getEstimatedBiases(null));
8501 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8502 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8503 assertNull(calibrator.getEstimatedBiasFx());
8504 assertNull(calibrator.getEstimatedBiasFy());
8505 assertNull(calibrator.getEstimatedBiasFz());
8506 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8507 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8508 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8509 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8510 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8511 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8512 assertNull(calibrator.getEstimatedMa());
8513 assertNull(calibrator.getEstimatedSx());
8514 assertNull(calibrator.getEstimatedSy());
8515 assertNull(calibrator.getEstimatedSz());
8516 assertNull(calibrator.getEstimatedMxy());
8517 assertNull(calibrator.getEstimatedMxz());
8518 assertNull(calibrator.getEstimatedMyx());
8519 assertNull(calibrator.getEstimatedMyz());
8520 assertNull(calibrator.getEstimatedMzx());
8521 assertNull(calibrator.getEstimatedMzy());
8522 assertNull(calibrator.getEstimatedCovariance());
8523 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8524 assertNull(calibrator.getGroundTruthGravityNorm());
8525 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8526 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8527
8528
8529 calibrator = null;
8530 try {
8531 calibrator = new KnownPositionAccelerometerCalibrator(
8532 measurements, new Matrix(1, 1), ma, this);
8533 fail("IllegalArgumentException expected but not thrown");
8534 } catch (final IllegalArgumentException ignore) {
8535 }
8536 try {
8537 calibrator = new KnownPositionAccelerometerCalibrator(
8538 measurements, new Matrix(1, 3), ma, this);
8539 fail("IllegalArgumentException expected but not thrown");
8540 } catch (final IllegalArgumentException ignore) {
8541 }
8542 try {
8543 calibrator = new KnownPositionAccelerometerCalibrator(
8544 measurements, ba, new Matrix(1, 3), this);
8545 fail("IllegalArgumentException expected but not thrown");
8546 } catch (final IllegalArgumentException ignore) {
8547 }
8548 try {
8549 calibrator = new KnownPositionAccelerometerCalibrator(
8550 measurements, ba, new Matrix(3, 1), this);
8551 fail("IllegalArgumentException expected but not thrown");
8552 } catch (final IllegalArgumentException ignore) {
8553 }
8554 assertNull(calibrator);
8555 }
8556
8557 @Test
8558 public void testConstructor75() throws WrongSizeException {
8559 final Matrix ba = generateBa();
8560 final double biasX = ba.getElementAtIndex(0);
8561 final double biasY = ba.getElementAtIndex(1);
8562 final double biasZ = ba.getElementAtIndex(2);
8563
8564 final Matrix ma = generateMaCommonAxis();
8565 final double sx = ma.getElementAt(0, 0);
8566 final double sy = ma.getElementAt(1, 1);
8567 final double sz = ma.getElementAt(2, 2);
8568 final double mxy = ma.getElementAt(0, 1);
8569 final double mxz = ma.getElementAt(0, 2);
8570 final double myx = ma.getElementAt(1, 0);
8571 final double myz = ma.getElementAt(1, 2);
8572 final double mzx = ma.getElementAt(2, 0);
8573 final double mzy = ma.getElementAt(2, 1);
8574
8575 KnownPositionAccelerometerCalibrator calibrator =
8576 new KnownPositionAccelerometerCalibrator(true, ba, ma);
8577
8578
8579 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8580 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8581 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8582 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8583 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8584 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8585 final Acceleration bx2 = new Acceleration(0.0,
8586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8587 calibrator.getInitialBiasXAsAcceleration(bx2);
8588 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8589 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8590 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8591 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8592 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8593 final Acceleration by2 = new Acceleration(0.0,
8594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8595 calibrator.getInitialBiasYAsAcceleration(by2);
8596 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8597 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8598 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8599 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8600 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8601 final Acceleration bz2 = new Acceleration(0.0,
8602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8603 calibrator.getInitialBiasZAsAcceleration(bz2);
8604 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8605 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8606 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8607 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8608 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8609 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8610 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8611 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8612 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8613 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8614 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8615 final double[] bias1 = calibrator.getInitialBias();
8616 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8617 final double[] bias2 = new double[3];
8618 calibrator.getInitialBias(bias2);
8619 assertArrayEquals(bias1, bias2, 0.0);
8620 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8621 assertEquals(b1, ba);
8622 final Matrix b2 = new Matrix(3, 1);
8623 calibrator.getInitialBiasAsMatrix(b2);
8624 assertEquals(b1, b2);
8625 final Matrix ma1 = new Matrix(3, 3);
8626 ma1.setSubmatrix(0, 0,
8627 2, 2,
8628 new double[]{sx, myx, mzx,
8629 mxy, sy, mzy,
8630 mxz, myz, sz});
8631 assertEquals(calibrator.getInitialMa(), ma1);
8632 final Matrix ma2 = new Matrix(3, 3);
8633 calibrator.getInitialMa(ma2);
8634 assertEquals(ma1, ma2);
8635 assertNull(calibrator.getEcefPosition());
8636 assertNull(calibrator.getNedPosition());
8637 assertFalse(calibrator.getNedPosition(null));
8638 assertNull(calibrator.getMeasurements());
8639 assertTrue(calibrator.isCommonAxisUsed());
8640 assertNull(calibrator.getListener());
8641 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8642 assertFalse(calibrator.isReady());
8643 assertFalse(calibrator.isRunning());
8644 assertNull(calibrator.getEstimatedBiases());
8645 assertFalse(calibrator.getEstimatedBiases(null));
8646 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8647 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8648 assertNull(calibrator.getEstimatedBiasFx());
8649 assertNull(calibrator.getEstimatedBiasFy());
8650 assertNull(calibrator.getEstimatedBiasFz());
8651 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8652 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8653 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8654 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8655 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8656 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8657 assertNull(calibrator.getEstimatedMa());
8658 assertNull(calibrator.getEstimatedSx());
8659 assertNull(calibrator.getEstimatedSy());
8660 assertNull(calibrator.getEstimatedSz());
8661 assertNull(calibrator.getEstimatedMxy());
8662 assertNull(calibrator.getEstimatedMxz());
8663 assertNull(calibrator.getEstimatedMyx());
8664 assertNull(calibrator.getEstimatedMyz());
8665 assertNull(calibrator.getEstimatedMzx());
8666 assertNull(calibrator.getEstimatedMzy());
8667 assertNull(calibrator.getEstimatedCovariance());
8668 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8669 assertNull(calibrator.getGroundTruthGravityNorm());
8670 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8671 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8672
8673
8674 calibrator = null;
8675 try {
8676 calibrator = new KnownPositionAccelerometerCalibrator(
8677 true, new Matrix(1, 1), ma);
8678 fail("IllegalArgumentException expected but not thrown");
8679 } catch (final IllegalArgumentException ignore) {
8680 }
8681 try {
8682 calibrator = new KnownPositionAccelerometerCalibrator(
8683 true, new Matrix(1, 3), ma);
8684 fail("IllegalArgumentException expected but not thrown");
8685 } catch (final IllegalArgumentException ignore) {
8686 }
8687 try {
8688 calibrator = new KnownPositionAccelerometerCalibrator(
8689 true, ba, new Matrix(1, 3));
8690 fail("IllegalArgumentException expected but not thrown");
8691 } catch (final IllegalArgumentException ignore) {
8692 }
8693 try {
8694 calibrator = new KnownPositionAccelerometerCalibrator(
8695 true, ba, new Matrix(3, 1));
8696 fail("IllegalArgumentException expected but not thrown");
8697 } catch (final IllegalArgumentException ignore) {
8698 }
8699 assertNull(calibrator);
8700 }
8701
8702 @Test
8703 public void testConstructor76() throws WrongSizeException {
8704 final Matrix ba = generateBa();
8705 final double biasX = ba.getElementAtIndex(0);
8706 final double biasY = ba.getElementAtIndex(1);
8707 final double biasZ = ba.getElementAtIndex(2);
8708
8709 final Matrix ma = generateMaCommonAxis();
8710 final double sx = ma.getElementAt(0, 0);
8711 final double sy = ma.getElementAt(1, 1);
8712 final double sz = ma.getElementAt(2, 2);
8713 final double mxy = ma.getElementAt(0, 1);
8714 final double mxz = ma.getElementAt(0, 2);
8715 final double myx = ma.getElementAt(1, 0);
8716 final double myz = ma.getElementAt(1, 2);
8717 final double mzx = ma.getElementAt(2, 0);
8718 final double mzy = ma.getElementAt(2, 1);
8719
8720 KnownPositionAccelerometerCalibrator calibrator =
8721 new KnownPositionAccelerometerCalibrator(true, ba, ma,
8722 this);
8723
8724
8725 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8726 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8727 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8728 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8729 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8730 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8731 final Acceleration bx2 = new Acceleration(0.0,
8732 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8733 calibrator.getInitialBiasXAsAcceleration(bx2);
8734 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8735 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8736 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8737 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8738 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8739 final Acceleration by2 = new Acceleration(0.0,
8740 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8741 calibrator.getInitialBiasYAsAcceleration(by2);
8742 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8743 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8744 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8745 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8746 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8747 final Acceleration bz2 = new Acceleration(0.0,
8748 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8749 calibrator.getInitialBiasZAsAcceleration(bz2);
8750 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8751 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8752 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8753 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8754 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8755 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8756 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8757 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8758 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8759 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8760 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8761 final double[] bias1 = calibrator.getInitialBias();
8762 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8763 final double[] bias2 = new double[3];
8764 calibrator.getInitialBias(bias2);
8765 assertArrayEquals(bias1, bias2, 0.0);
8766 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8767 assertEquals(b1, ba);
8768 final Matrix b2 = new Matrix(3, 1);
8769 calibrator.getInitialBiasAsMatrix(b2);
8770 assertEquals(b1, b2);
8771 final Matrix ma1 = new Matrix(3, 3);
8772 ma1.setSubmatrix(0, 0,
8773 2, 2,
8774 new double[]{sx, myx, mzx,
8775 mxy, sy, mzy,
8776 mxz, myz, sz});
8777 assertEquals(calibrator.getInitialMa(), ma1);
8778 final Matrix ma2 = new Matrix(3, 3);
8779 calibrator.getInitialMa(ma2);
8780 assertEquals(ma1, ma2);
8781 assertNull(calibrator.getEcefPosition());
8782 assertNull(calibrator.getNedPosition());
8783 assertFalse(calibrator.getNedPosition(null));
8784 assertNull(calibrator.getMeasurements());
8785 assertTrue(calibrator.isCommonAxisUsed());
8786 assertSame(calibrator.getListener(), this);
8787 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8788 assertFalse(calibrator.isReady());
8789 assertFalse(calibrator.isRunning());
8790 assertNull(calibrator.getEstimatedBiases());
8791 assertFalse(calibrator.getEstimatedBiases(null));
8792 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8793 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8794 assertNull(calibrator.getEstimatedBiasFx());
8795 assertNull(calibrator.getEstimatedBiasFy());
8796 assertNull(calibrator.getEstimatedBiasFz());
8797 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8798 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8799 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8800 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8801 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8802 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8803 assertNull(calibrator.getEstimatedMa());
8804 assertNull(calibrator.getEstimatedSx());
8805 assertNull(calibrator.getEstimatedSy());
8806 assertNull(calibrator.getEstimatedSz());
8807 assertNull(calibrator.getEstimatedMxy());
8808 assertNull(calibrator.getEstimatedMxz());
8809 assertNull(calibrator.getEstimatedMyx());
8810 assertNull(calibrator.getEstimatedMyz());
8811 assertNull(calibrator.getEstimatedMzx());
8812 assertNull(calibrator.getEstimatedMzy());
8813 assertNull(calibrator.getEstimatedCovariance());
8814 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8815 assertNull(calibrator.getGroundTruthGravityNorm());
8816 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8817 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8818
8819
8820 calibrator = null;
8821 try {
8822 calibrator = new KnownPositionAccelerometerCalibrator(
8823 true, new Matrix(1, 1), ma,
8824 this);
8825 fail("IllegalArgumentException expected but not thrown");
8826 } catch (final IllegalArgumentException ignore) {
8827 }
8828 try {
8829 calibrator = new KnownPositionAccelerometerCalibrator(
8830 true, new Matrix(1, 3), ma,
8831 this);
8832 fail("IllegalArgumentException expected but not thrown");
8833 } catch (final IllegalArgumentException ignore) {
8834 }
8835 try {
8836 calibrator = new KnownPositionAccelerometerCalibrator(
8837 true, ba, new Matrix(1, 3),
8838 this);
8839 fail("IllegalArgumentException expected but not thrown");
8840 } catch (final IllegalArgumentException ignore) {
8841 }
8842 try {
8843 calibrator = new KnownPositionAccelerometerCalibrator(
8844 true, ba, new Matrix(3, 1),
8845 this);
8846 fail("IllegalArgumentException expected but not thrown");
8847 } catch (final IllegalArgumentException ignore) {
8848 }
8849 assertNull(calibrator);
8850 }
8851
8852 @Test
8853 public void testConstructor77() throws WrongSizeException {
8854 final Collection<StandardDeviationBodyKinematics> measurements =
8855 Collections.emptyList();
8856
8857 final Matrix ba = generateBa();
8858 final double biasX = ba.getElementAtIndex(0);
8859 final double biasY = ba.getElementAtIndex(1);
8860 final double biasZ = ba.getElementAtIndex(2);
8861
8862 final Matrix ma = generateMaCommonAxis();
8863 final double sx = ma.getElementAt(0, 0);
8864 final double sy = ma.getElementAt(1, 1);
8865 final double sz = ma.getElementAt(2, 2);
8866 final double mxy = ma.getElementAt(0, 1);
8867 final double mxz = ma.getElementAt(0, 2);
8868 final double myx = ma.getElementAt(1, 0);
8869 final double myz = ma.getElementAt(1, 2);
8870 final double mzx = ma.getElementAt(2, 0);
8871 final double mzy = ma.getElementAt(2, 1);
8872
8873 KnownPositionAccelerometerCalibrator calibrator =
8874 new KnownPositionAccelerometerCalibrator(measurements,
8875 true, ba, ma);
8876
8877
8878 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8879 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8880 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8881 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8882 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8883 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8884 final Acceleration bx2 = new Acceleration(0.0,
8885 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8886 calibrator.getInitialBiasXAsAcceleration(bx2);
8887 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8888 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8889 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8890 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8891 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8892 final Acceleration by2 = new Acceleration(0.0,
8893 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8894 calibrator.getInitialBiasYAsAcceleration(by2);
8895 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8896 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8897 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8898 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8899 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8900 final Acceleration bz2 = new Acceleration(0.0,
8901 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8902 calibrator.getInitialBiasZAsAcceleration(bz2);
8903 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8904 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8905 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8906 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8907 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8908 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8909 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8910 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8911 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8912 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8913 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8914 final double[] bias1 = calibrator.getInitialBias();
8915 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8916 final double[] bias2 = new double[3];
8917 calibrator.getInitialBias(bias2);
8918 assertArrayEquals(bias1, bias2, 0.0);
8919 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8920 assertEquals(b1, ba);
8921 final Matrix b2 = new Matrix(3, 1);
8922 calibrator.getInitialBiasAsMatrix(b2);
8923 assertEquals(b1, b2);
8924 final Matrix ma1 = new Matrix(3, 3);
8925 ma1.setSubmatrix(0, 0,
8926 2, 2,
8927 new double[]{sx, myx, mzx,
8928 mxy, sy, mzy,
8929 mxz, myz, sz});
8930 assertEquals(calibrator.getInitialMa(), ma1);
8931 final Matrix ma2 = new Matrix(3, 3);
8932 calibrator.getInitialMa(ma2);
8933 assertEquals(ma1, ma2);
8934 assertNull(calibrator.getEcefPosition());
8935 assertNull(calibrator.getNedPosition());
8936 assertFalse(calibrator.getNedPosition(null));
8937 assertSame(calibrator.getMeasurements(), measurements);
8938 assertTrue(calibrator.isCommonAxisUsed());
8939 assertNull(calibrator.getListener());
8940 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8941 assertFalse(calibrator.isReady());
8942 assertFalse(calibrator.isRunning());
8943 assertNull(calibrator.getEstimatedBiases());
8944 assertFalse(calibrator.getEstimatedBiases(null));
8945 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8946 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8947 assertNull(calibrator.getEstimatedBiasFx());
8948 assertNull(calibrator.getEstimatedBiasFy());
8949 assertNull(calibrator.getEstimatedBiasFz());
8950 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8951 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8952 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8953 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8954 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8955 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8956 assertNull(calibrator.getEstimatedMa());
8957 assertNull(calibrator.getEstimatedSx());
8958 assertNull(calibrator.getEstimatedSy());
8959 assertNull(calibrator.getEstimatedSz());
8960 assertNull(calibrator.getEstimatedMxy());
8961 assertNull(calibrator.getEstimatedMxz());
8962 assertNull(calibrator.getEstimatedMyx());
8963 assertNull(calibrator.getEstimatedMyz());
8964 assertNull(calibrator.getEstimatedMzx());
8965 assertNull(calibrator.getEstimatedMzy());
8966 assertNull(calibrator.getEstimatedCovariance());
8967 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8968 assertNull(calibrator.getGroundTruthGravityNorm());
8969 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8970 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8971
8972
8973 calibrator = null;
8974 try {
8975 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8976 true, new Matrix(1, 1), ma);
8977 fail("IllegalArgumentException expected but not thrown");
8978 } catch (final IllegalArgumentException ignore) {
8979 }
8980 try {
8981 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8982 true, new Matrix(1, 3), ma);
8983 fail("IllegalArgumentException expected but not thrown");
8984 } catch (final IllegalArgumentException ignore) {
8985 }
8986 try {
8987 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8988 true, ba, new Matrix(1, 3));
8989 fail("IllegalArgumentException expected but not thrown");
8990 } catch (final IllegalArgumentException ignore) {
8991 }
8992 try {
8993 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8994 true, ba, new Matrix(3, 1));
8995 fail("IllegalArgumentException expected but not thrown");
8996 } catch (final IllegalArgumentException ignore) {
8997 }
8998 assertNull(calibrator);
8999 }
9000
9001 @Test
9002 public void testConstructor78() throws WrongSizeException {
9003 final Collection<StandardDeviationBodyKinematics> measurements =
9004 Collections.emptyList();
9005
9006 final Matrix ba = generateBa();
9007 final double biasX = ba.getElementAtIndex(0);
9008 final double biasY = ba.getElementAtIndex(1);
9009 final double biasZ = ba.getElementAtIndex(2);
9010
9011 final Matrix ma = generateMaCommonAxis();
9012 final double sx = ma.getElementAt(0, 0);
9013 final double sy = ma.getElementAt(1, 1);
9014 final double sz = ma.getElementAt(2, 2);
9015 final double mxy = ma.getElementAt(0, 1);
9016 final double mxz = ma.getElementAt(0, 2);
9017 final double myx = ma.getElementAt(1, 0);
9018 final double myz = ma.getElementAt(1, 2);
9019 final double mzx = ma.getElementAt(2, 0);
9020 final double mzy = ma.getElementAt(2, 1);
9021
9022 KnownPositionAccelerometerCalibrator calibrator =
9023 new KnownPositionAccelerometerCalibrator(measurements,
9024 true, ba, ma, this);
9025
9026
9027 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9028 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9029 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9030 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9031 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9032 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9033 final Acceleration bx2 = new Acceleration(0.0,
9034 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9035 calibrator.getInitialBiasXAsAcceleration(bx2);
9036 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9037 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9038 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9039 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9040 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9041 final Acceleration by2 = new Acceleration(0.0,
9042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9043 calibrator.getInitialBiasYAsAcceleration(by2);
9044 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9045 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9046 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9047 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9048 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9049 final Acceleration bz2 = new Acceleration(0.0,
9050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9051 calibrator.getInitialBiasZAsAcceleration(bz2);
9052 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9053 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9054 assertEquals(calibrator.getInitialSx(), sx, 0.0);
9055 assertEquals(calibrator.getInitialSy(), sy, 0.0);
9056 assertEquals(calibrator.getInitialSz(), sz, 0.0);
9057 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
9058 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
9059 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
9060 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
9061 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
9062 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
9063 final double[] bias1 = calibrator.getInitialBias();
9064 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9065 final double[] bias2 = new double[3];
9066 calibrator.getInitialBias(bias2);
9067 assertArrayEquals(bias1, bias2, 0.0);
9068 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9069 assertEquals(b1, ba);
9070 final Matrix b2 = new Matrix(3, 1);
9071 calibrator.getInitialBiasAsMatrix(b2);
9072 assertEquals(b1, b2);
9073 final Matrix ma1 = new Matrix(3, 3);
9074 ma1.setSubmatrix(0, 0,
9075 2, 2,
9076 new double[]{sx, myx, mzx,
9077 mxy, sy, mzy,
9078 mxz, myz, sz});
9079 assertEquals(calibrator.getInitialMa(), ma1);
9080 final Matrix ma2 = new Matrix(3, 3);
9081 calibrator.getInitialMa(ma2);
9082 assertEquals(ma1, ma2);
9083 assertNull(calibrator.getEcefPosition());
9084 assertNull(calibrator.getNedPosition());
9085 assertFalse(calibrator.getNedPosition(null));
9086 assertSame(calibrator.getMeasurements(), measurements);
9087 assertTrue(calibrator.isCommonAxisUsed());
9088 assertSame(calibrator.getListener(), this);
9089 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9090 assertFalse(calibrator.isReady());
9091 assertFalse(calibrator.isRunning());
9092 assertNull(calibrator.getEstimatedBiases());
9093 assertFalse(calibrator.getEstimatedBiases(null));
9094 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9095 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9096 assertNull(calibrator.getEstimatedBiasFx());
9097 assertNull(calibrator.getEstimatedBiasFy());
9098 assertNull(calibrator.getEstimatedBiasFz());
9099 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9100 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9101 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9102 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9103 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9104 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9105 assertNull(calibrator.getEstimatedMa());
9106 assertNull(calibrator.getEstimatedSx());
9107 assertNull(calibrator.getEstimatedSy());
9108 assertNull(calibrator.getEstimatedSz());
9109 assertNull(calibrator.getEstimatedMxy());
9110 assertNull(calibrator.getEstimatedMxz());
9111 assertNull(calibrator.getEstimatedMyx());
9112 assertNull(calibrator.getEstimatedMyz());
9113 assertNull(calibrator.getEstimatedMzx());
9114 assertNull(calibrator.getEstimatedMzy());
9115 assertNull(calibrator.getEstimatedCovariance());
9116 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9117 assertNull(calibrator.getGroundTruthGravityNorm());
9118 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9119 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
9120
9121
9122 calibrator = null;
9123 try {
9124 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9125 true, new Matrix(1, 1), ma,
9126 this);
9127 fail("IllegalArgumentException expected but not thrown");
9128 } catch (final IllegalArgumentException ignore) {
9129 }
9130 try {
9131 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9132 true, new Matrix(1, 3), ma,
9133 this);
9134 fail("IllegalArgumentException expected but not thrown");
9135 } catch (final IllegalArgumentException ignore) {
9136 }
9137 try {
9138 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9139 true, ba, new Matrix(1, 3),
9140 this);
9141 fail("IllegalArgumentException expected but not thrown");
9142 } catch (final IllegalArgumentException ignore) {
9143 }
9144 try {
9145 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9146 true, ba, new Matrix(3, 1),
9147 this);
9148 fail("IllegalArgumentException expected but not thrown");
9149 } catch (final IllegalArgumentException ignore) {
9150 }
9151 assertNull(calibrator);
9152 }
9153
9154 @Test
9155 public void testConstructor79() throws WrongSizeException {
9156 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9157 final double latitude = Math.toRadians(
9158 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9159 final double longitude = Math.toRadians(
9160 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9161 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9162 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9163 final NEDVelocity nedVelocity = new NEDVelocity();
9164 final ECEFPosition ecefPosition = new ECEFPosition();
9165 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9166 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9167 ecefPosition, ecefVelocity);
9168
9169 final KnownPositionAccelerometerCalibrator calibrator =
9170 new KnownPositionAccelerometerCalibrator(ecefPosition);
9171
9172
9173 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9174 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9175 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9176 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9177 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9178 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9179 final Acceleration bx2 = new Acceleration(0.0,
9180 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9181 calibrator.getInitialBiasXAsAcceleration(bx2);
9182 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9183 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9184 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9185 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9186 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9187 final Acceleration by2 = new Acceleration(0.0,
9188 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9189 calibrator.getInitialBiasYAsAcceleration(by2);
9190 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9191 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9192 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9193 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9194 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9195 final Acceleration bz2 = new Acceleration(0.0,
9196 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9197 calibrator.getInitialBiasZAsAcceleration(bz2);
9198 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9199 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9200 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9201 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9202 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9203 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9204 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9205 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9206 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9207 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9208 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9209 final double[] bias1 = calibrator.getInitialBias();
9210 assertArrayEquals(bias1, new double[3], 0.0);
9211 final double[] bias2 = new double[3];
9212 calibrator.getInitialBias(bias2);
9213 assertArrayEquals(bias1, bias2, 0.0);
9214 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9215 assertEquals(b1, new Matrix(3, 1));
9216 final Matrix b2 = new Matrix(3, 1);
9217 calibrator.getInitialBiasAsMatrix(b2);
9218 assertEquals(b1, b2);
9219 final Matrix ma1 = calibrator.getInitialMa();
9220 assertEquals(ma1, new Matrix(3, 3));
9221 final Matrix ma2 = new Matrix(3, 3);
9222 calibrator.getInitialMa(ma2);
9223 assertEquals(ma1, ma2);
9224 assertSame(calibrator.getEcefPosition(), ecefPosition);
9225 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9226 final NEDPosition nedPosition1 = new NEDPosition();
9227 assertTrue(calibrator.getNedPosition(nedPosition1));
9228 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9229 assertNull(calibrator.getMeasurements());
9230 assertFalse(calibrator.isCommonAxisUsed());
9231 assertNull(calibrator.getListener());
9232 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9233 assertFalse(calibrator.isReady());
9234 assertFalse(calibrator.isRunning());
9235 assertNull(calibrator.getEstimatedBiases());
9236 assertFalse(calibrator.getEstimatedBiases(null));
9237 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9238 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9239 assertNull(calibrator.getEstimatedBiasFx());
9240 assertNull(calibrator.getEstimatedBiasFy());
9241 assertNull(calibrator.getEstimatedBiasFz());
9242 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9243 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9244 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9245 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9246 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9247 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9248 assertNull(calibrator.getEstimatedMa());
9249 assertNull(calibrator.getEstimatedSx());
9250 assertNull(calibrator.getEstimatedSy());
9251 assertNull(calibrator.getEstimatedSz());
9252 assertNull(calibrator.getEstimatedMxy());
9253 assertNull(calibrator.getEstimatedMxz());
9254 assertNull(calibrator.getEstimatedMyx());
9255 assertNull(calibrator.getEstimatedMyz());
9256 assertNull(calibrator.getEstimatedMzx());
9257 assertNull(calibrator.getEstimatedMzy());
9258 assertNull(calibrator.getEstimatedCovariance());
9259 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9260 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9261 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9262 assertNotNull(calibrator.getGroundTruthGravityNorm());
9263 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9264 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9265 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9266 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9267 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9268 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9269 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9270 }
9271
9272 @Test
9273 public void testConstructor80() throws WrongSizeException {
9274 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9275 final double latitude = Math.toRadians(
9276 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9277 final double longitude = Math.toRadians(
9278 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9279 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9280 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9281 final NEDVelocity nedVelocity = new NEDVelocity();
9282 final ECEFPosition ecefPosition = new ECEFPosition();
9283 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9284 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9285 ecefPosition, ecefVelocity);
9286
9287 final KnownPositionAccelerometerCalibrator calibrator =
9288 new KnownPositionAccelerometerCalibrator(ecefPosition, this);
9289
9290
9291 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9292 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9293 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9294 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9295 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9296 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9297 final Acceleration bx2 = new Acceleration(0.0,
9298 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9299 calibrator.getInitialBiasXAsAcceleration(bx2);
9300 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9301 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9302 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9303 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9304 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9305 final Acceleration by2 = new Acceleration(0.0,
9306 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9307 calibrator.getInitialBiasYAsAcceleration(by2);
9308 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9309 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9310 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9311 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9312 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9313 final Acceleration bz2 = new Acceleration(0.0,
9314 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9315 calibrator.getInitialBiasZAsAcceleration(bz2);
9316 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9317 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9318 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9319 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9320 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9321 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9322 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9323 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9324 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9325 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9326 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9327 final double[] bias1 = calibrator.getInitialBias();
9328 assertArrayEquals(bias1, new double[3], 0.0);
9329 final double[] bias2 = new double[3];
9330 calibrator.getInitialBias(bias2);
9331 assertArrayEquals(bias1, bias2, 0.0);
9332 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9333 assertEquals(b1, new Matrix(3, 1));
9334 final Matrix b2 = new Matrix(3, 1);
9335 calibrator.getInitialBiasAsMatrix(b2);
9336 assertEquals(b1, b2);
9337 final Matrix ma1 = calibrator.getInitialMa();
9338 assertEquals(ma1, new Matrix(3, 3));
9339 final Matrix ma2 = new Matrix(3, 3);
9340 calibrator.getInitialMa(ma2);
9341 assertEquals(ma1, ma2);
9342 assertSame(calibrator.getEcefPosition(), ecefPosition);
9343 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9344 final NEDPosition nedPosition1 = new NEDPosition();
9345 assertTrue(calibrator.getNedPosition(nedPosition1));
9346 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9347 assertNull(calibrator.getMeasurements());
9348 assertFalse(calibrator.isCommonAxisUsed());
9349 assertSame(calibrator.getListener(), this);
9350 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9351 assertFalse(calibrator.isReady());
9352 assertFalse(calibrator.isRunning());
9353 assertNull(calibrator.getEstimatedBiases());
9354 assertFalse(calibrator.getEstimatedBiases(null));
9355 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9356 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9357 assertNull(calibrator.getEstimatedBiasFx());
9358 assertNull(calibrator.getEstimatedBiasFy());
9359 assertNull(calibrator.getEstimatedBiasFz());
9360 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9361 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9362 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9363 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9364 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9365 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9366 assertNull(calibrator.getEstimatedMa());
9367 assertNull(calibrator.getEstimatedSx());
9368 assertNull(calibrator.getEstimatedSy());
9369 assertNull(calibrator.getEstimatedSz());
9370 assertNull(calibrator.getEstimatedMxy());
9371 assertNull(calibrator.getEstimatedMxz());
9372 assertNull(calibrator.getEstimatedMyx());
9373 assertNull(calibrator.getEstimatedMyz());
9374 assertNull(calibrator.getEstimatedMzx());
9375 assertNull(calibrator.getEstimatedMzy());
9376 assertNull(calibrator.getEstimatedCovariance());
9377 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9378 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9379 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9380 assertNotNull(calibrator.getGroundTruthGravityNorm());
9381 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9382 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9383 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9384 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9385 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9386 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9387 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9388 }
9389
9390 @Test
9391 public void testConstructor81() throws WrongSizeException {
9392 final Collection<StandardDeviationBodyKinematics> measurements =
9393 Collections.emptyList();
9394
9395 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9396 final double latitude = Math.toRadians(
9397 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9398 final double longitude = Math.toRadians(
9399 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9400 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9401 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9402 final NEDVelocity nedVelocity = new NEDVelocity();
9403 final ECEFPosition ecefPosition = new ECEFPosition();
9404 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9405 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9406 ecefPosition, ecefVelocity);
9407
9408 final KnownPositionAccelerometerCalibrator calibrator =
9409 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements);
9410
9411
9412 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9413 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9414 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9415 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9416 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9417 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9418 final Acceleration bx2 = new Acceleration(0.0,
9419 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9420 calibrator.getInitialBiasXAsAcceleration(bx2);
9421 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9422 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9423 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9424 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9425 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9426 final Acceleration by2 = new Acceleration(0.0,
9427 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9428 calibrator.getInitialBiasYAsAcceleration(by2);
9429 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9430 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9431 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9432 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9433 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9434 final Acceleration bz2 = new Acceleration(0.0,
9435 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9436 calibrator.getInitialBiasZAsAcceleration(bz2);
9437 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9438 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9439 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9440 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9441 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9442 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9443 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9444 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9445 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9446 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9447 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9448 final double[] bias1 = calibrator.getInitialBias();
9449 assertArrayEquals(bias1, new double[3], 0.0);
9450 final double[] bias2 = new double[3];
9451 calibrator.getInitialBias(bias2);
9452 assertArrayEquals(bias1, bias2, 0.0);
9453 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9454 assertEquals(b1, new Matrix(3, 1));
9455 final Matrix b2 = new Matrix(3, 1);
9456 calibrator.getInitialBiasAsMatrix(b2);
9457 assertEquals(b1, b2);
9458 final Matrix ma1 = calibrator.getInitialMa();
9459 assertEquals(ma1, new Matrix(3, 3));
9460 final Matrix ma2 = new Matrix(3, 3);
9461 calibrator.getInitialMa(ma2);
9462 assertEquals(ma1, ma2);
9463 assertSame(calibrator.getEcefPosition(), ecefPosition);
9464 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9465 final NEDPosition nedPosition1 = new NEDPosition();
9466 assertTrue(calibrator.getNedPosition(nedPosition1));
9467 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9468 assertSame(calibrator.getMeasurements(), measurements);
9469 assertFalse(calibrator.isCommonAxisUsed());
9470 assertNull(calibrator.getListener());
9471 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9472 assertFalse(calibrator.isReady());
9473 assertFalse(calibrator.isRunning());
9474 assertNull(calibrator.getEstimatedBiases());
9475 assertFalse(calibrator.getEstimatedBiases(null));
9476 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9477 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9478 assertNull(calibrator.getEstimatedBiasFx());
9479 assertNull(calibrator.getEstimatedBiasFy());
9480 assertNull(calibrator.getEstimatedBiasFz());
9481 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9482 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9483 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9484 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9485 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9486 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9487 assertNull(calibrator.getEstimatedMa());
9488 assertNull(calibrator.getEstimatedSx());
9489 assertNull(calibrator.getEstimatedSy());
9490 assertNull(calibrator.getEstimatedSz());
9491 assertNull(calibrator.getEstimatedMxy());
9492 assertNull(calibrator.getEstimatedMxz());
9493 assertNull(calibrator.getEstimatedMyx());
9494 assertNull(calibrator.getEstimatedMyz());
9495 assertNull(calibrator.getEstimatedMzx());
9496 assertNull(calibrator.getEstimatedMzy());
9497 assertNull(calibrator.getEstimatedCovariance());
9498 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9499 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9500 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9501 assertNotNull(calibrator.getGroundTruthGravityNorm());
9502 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9503 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9504 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9505 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9506 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9507 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9508 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9509 }
9510
9511 @Test
9512 public void testConstructor82() throws WrongSizeException {
9513 final Collection<StandardDeviationBodyKinematics> measurements =
9514 Collections.emptyList();
9515
9516 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9517 final double latitude = Math.toRadians(
9518 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9519 final double longitude = Math.toRadians(
9520 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9521 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9522 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9523 final NEDVelocity nedVelocity = new NEDVelocity();
9524 final ECEFPosition ecefPosition = new ECEFPosition();
9525 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9526 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9527 ecefPosition, ecefVelocity);
9528
9529 final KnownPositionAccelerometerCalibrator calibrator =
9530 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9531 this);
9532
9533
9534 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9535 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9536 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9537 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9538 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9539 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9540 final Acceleration bx2 = new Acceleration(0.0,
9541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9542 calibrator.getInitialBiasXAsAcceleration(bx2);
9543 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9544 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9545 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9546 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9547 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9548 final Acceleration by2 = new Acceleration(0.0,
9549 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9550 calibrator.getInitialBiasYAsAcceleration(by2);
9551 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9552 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9553 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9554 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9555 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9556 final Acceleration bz2 = new Acceleration(0.0,
9557 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9558 calibrator.getInitialBiasZAsAcceleration(bz2);
9559 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9560 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9561 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9562 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9563 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9564 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9565 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9566 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9567 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9568 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9569 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9570 final double[] bias1 = calibrator.getInitialBias();
9571 assertArrayEquals(bias1, new double[3], 0.0);
9572 final double[] bias2 = new double[3];
9573 calibrator.getInitialBias(bias2);
9574 assertArrayEquals(bias1, bias2, 0.0);
9575 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9576 assertEquals(b1, new Matrix(3, 1));
9577 final Matrix b2 = new Matrix(3, 1);
9578 calibrator.getInitialBiasAsMatrix(b2);
9579 assertEquals(b1, b2);
9580 final Matrix ma1 = calibrator.getInitialMa();
9581 assertEquals(ma1, new Matrix(3, 3));
9582 final Matrix ma2 = new Matrix(3, 3);
9583 calibrator.getInitialMa(ma2);
9584 assertEquals(ma1, ma2);
9585 assertSame(calibrator.getEcefPosition(), ecefPosition);
9586 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9587 final NEDPosition nedPosition1 = new NEDPosition();
9588 assertTrue(calibrator.getNedPosition(nedPosition1));
9589 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9590 assertSame(calibrator.getMeasurements(), measurements);
9591 assertFalse(calibrator.isCommonAxisUsed());
9592 assertSame(calibrator.getListener(), this);
9593 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9594 assertFalse(calibrator.isReady());
9595 assertFalse(calibrator.isRunning());
9596 assertNull(calibrator.getEstimatedBiases());
9597 assertFalse(calibrator.getEstimatedBiases(null));
9598 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9599 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9600 assertNull(calibrator.getEstimatedBiasFx());
9601 assertNull(calibrator.getEstimatedBiasFy());
9602 assertNull(calibrator.getEstimatedBiasFz());
9603 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9604 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9605 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9606 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9607 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9608 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9609 assertNull(calibrator.getEstimatedMa());
9610 assertNull(calibrator.getEstimatedSx());
9611 assertNull(calibrator.getEstimatedSy());
9612 assertNull(calibrator.getEstimatedSz());
9613 assertNull(calibrator.getEstimatedMxy());
9614 assertNull(calibrator.getEstimatedMxz());
9615 assertNull(calibrator.getEstimatedMyx());
9616 assertNull(calibrator.getEstimatedMyz());
9617 assertNull(calibrator.getEstimatedMzx());
9618 assertNull(calibrator.getEstimatedMzy());
9619 assertNull(calibrator.getEstimatedCovariance());
9620 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9621 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9622 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9623 assertNotNull(calibrator.getGroundTruthGravityNorm());
9624 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9625 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9626 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9627 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9628 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9629 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9630 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9631 }
9632
9633 @Test
9634 public void testConstructor83() throws WrongSizeException {
9635 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9636 final double latitude = Math.toRadians(
9637 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9638 final double longitude = Math.toRadians(
9639 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9640 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9641 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9642 final NEDVelocity nedVelocity = new NEDVelocity();
9643 final ECEFPosition ecefPosition = new ECEFPosition();
9644 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9645 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9646 ecefPosition, ecefVelocity);
9647
9648 final KnownPositionAccelerometerCalibrator calibrator =
9649 new KnownPositionAccelerometerCalibrator(ecefPosition,
9650 true);
9651
9652
9653 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9654 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9655 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9656 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9657 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9658 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9659 final Acceleration bx2 = new Acceleration(0.0,
9660 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9661 calibrator.getInitialBiasXAsAcceleration(bx2);
9662 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9663 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9664 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9665 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9666 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9667 final Acceleration by2 = new Acceleration(0.0,
9668 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9669 calibrator.getInitialBiasYAsAcceleration(by2);
9670 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9671 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9672 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9673 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9674 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9675 final Acceleration bz2 = new Acceleration(0.0,
9676 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9677 calibrator.getInitialBiasZAsAcceleration(bz2);
9678 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9679 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9680 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9681 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9682 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9683 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9684 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9685 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9686 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9687 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9688 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9689 final double[] bias1 = calibrator.getInitialBias();
9690 assertArrayEquals(bias1, new double[3], 0.0);
9691 final double[] bias2 = new double[3];
9692 calibrator.getInitialBias(bias2);
9693 assertArrayEquals(bias1, bias2, 0.0);
9694 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9695 assertEquals(b1, new Matrix(3, 1));
9696 final Matrix b2 = new Matrix(3, 1);
9697 calibrator.getInitialBiasAsMatrix(b2);
9698 assertEquals(b1, b2);
9699 final Matrix ma1 = calibrator.getInitialMa();
9700 assertEquals(ma1, new Matrix(3, 3));
9701 final Matrix ma2 = new Matrix(3, 3);
9702 calibrator.getInitialMa(ma2);
9703 assertEquals(ma1, ma2);
9704 assertSame(calibrator.getEcefPosition(), ecefPosition);
9705 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9706 final NEDPosition nedPosition1 = new NEDPosition();
9707 assertTrue(calibrator.getNedPosition(nedPosition1));
9708 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9709 assertNull(calibrator.getMeasurements());
9710 assertTrue(calibrator.isCommonAxisUsed());
9711 assertNull(calibrator.getListener());
9712 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9713 assertFalse(calibrator.isReady());
9714 assertFalse(calibrator.isRunning());
9715 assertNull(calibrator.getEstimatedBiases());
9716 assertFalse(calibrator.getEstimatedBiases(null));
9717 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9718 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9719 assertNull(calibrator.getEstimatedBiasFx());
9720 assertNull(calibrator.getEstimatedBiasFy());
9721 assertNull(calibrator.getEstimatedBiasFz());
9722 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9723 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9724 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9725 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9726 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9727 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9728 assertNull(calibrator.getEstimatedMa());
9729 assertNull(calibrator.getEstimatedSx());
9730 assertNull(calibrator.getEstimatedSy());
9731 assertNull(calibrator.getEstimatedSz());
9732 assertNull(calibrator.getEstimatedMxy());
9733 assertNull(calibrator.getEstimatedMxz());
9734 assertNull(calibrator.getEstimatedMyx());
9735 assertNull(calibrator.getEstimatedMyz());
9736 assertNull(calibrator.getEstimatedMzx());
9737 assertNull(calibrator.getEstimatedMzy());
9738 assertNull(calibrator.getEstimatedCovariance());
9739 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9740 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9741 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9742 assertNotNull(calibrator.getGroundTruthGravityNorm());
9743 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9744 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9745 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9746 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9747 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9748 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9749 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9750 }
9751
9752 @Test
9753 public void testConstructor84() throws WrongSizeException {
9754 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9755 final double latitude = Math.toRadians(
9756 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9757 final double longitude = Math.toRadians(
9758 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9759 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9760 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9761 final NEDVelocity nedVelocity = new NEDVelocity();
9762 final ECEFPosition ecefPosition = new ECEFPosition();
9763 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9764 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9765 ecefPosition, ecefVelocity);
9766
9767 final KnownPositionAccelerometerCalibrator calibrator =
9768 new KnownPositionAccelerometerCalibrator(ecefPosition,
9769 true, this);
9770
9771
9772 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9773 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9774 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9775 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9776 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9777 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9778 final Acceleration bx2 = new Acceleration(0.0,
9779 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9780 calibrator.getInitialBiasXAsAcceleration(bx2);
9781 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9782 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9783 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9784 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9785 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9786 final Acceleration by2 = new Acceleration(0.0,
9787 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9788 calibrator.getInitialBiasYAsAcceleration(by2);
9789 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9790 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9791 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9792 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9793 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9794 final Acceleration bz2 = new Acceleration(0.0,
9795 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9796 calibrator.getInitialBiasZAsAcceleration(bz2);
9797 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9798 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9799 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9800 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9801 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9802 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9803 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9804 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9805 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9806 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9807 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9808 final double[] bias1 = calibrator.getInitialBias();
9809 assertArrayEquals(bias1, new double[3], 0.0);
9810 final double[] bias2 = new double[3];
9811 calibrator.getInitialBias(bias2);
9812 assertArrayEquals(bias1, bias2, 0.0);
9813 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9814 assertEquals(b1, new Matrix(3, 1));
9815 final Matrix b2 = new Matrix(3, 1);
9816 calibrator.getInitialBiasAsMatrix(b2);
9817 assertEquals(b1, b2);
9818 final Matrix ma1 = calibrator.getInitialMa();
9819 assertEquals(ma1, new Matrix(3, 3));
9820 final Matrix ma2 = new Matrix(3, 3);
9821 calibrator.getInitialMa(ma2);
9822 assertEquals(ma1, ma2);
9823 assertSame(calibrator.getEcefPosition(), ecefPosition);
9824 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9825 final NEDPosition nedPosition1 = new NEDPosition();
9826 assertTrue(calibrator.getNedPosition(nedPosition1));
9827 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9828 assertNull(calibrator.getMeasurements());
9829 assertTrue(calibrator.isCommonAxisUsed());
9830 assertSame(calibrator.getListener(), this);
9831 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9832 assertFalse(calibrator.isReady());
9833 assertFalse(calibrator.isRunning());
9834 assertNull(calibrator.getEstimatedBiases());
9835 assertFalse(calibrator.getEstimatedBiases(null));
9836 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9837 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9838 assertNull(calibrator.getEstimatedBiasFx());
9839 assertNull(calibrator.getEstimatedBiasFy());
9840 assertNull(calibrator.getEstimatedBiasFz());
9841 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9842 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9843 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9844 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9845 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9846 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9847 assertNull(calibrator.getEstimatedMa());
9848 assertNull(calibrator.getEstimatedSx());
9849 assertNull(calibrator.getEstimatedSy());
9850 assertNull(calibrator.getEstimatedSz());
9851 assertNull(calibrator.getEstimatedMxy());
9852 assertNull(calibrator.getEstimatedMxz());
9853 assertNull(calibrator.getEstimatedMyx());
9854 assertNull(calibrator.getEstimatedMyz());
9855 assertNull(calibrator.getEstimatedMzx());
9856 assertNull(calibrator.getEstimatedMzy());
9857 assertNull(calibrator.getEstimatedCovariance());
9858 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9859 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9860 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9861 assertNotNull(calibrator.getGroundTruthGravityNorm());
9862 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9863 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9864 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9865 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9866 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9867 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9868 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9869 }
9870
9871 @Test
9872 public void testConstructor85() throws WrongSizeException {
9873 final Collection<StandardDeviationBodyKinematics> measurements =
9874 Collections.emptyList();
9875 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9876 final double latitude = Math.toRadians(
9877 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9878 final double longitude = Math.toRadians(
9879 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9880 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9881 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9882 final NEDVelocity nedVelocity = new NEDVelocity();
9883 final ECEFPosition ecefPosition = new ECEFPosition();
9884 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9885 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9886 ecefPosition, ecefVelocity);
9887
9888 final KnownPositionAccelerometerCalibrator calibrator =
9889 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9890 true);
9891
9892
9893 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9894 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9895 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9896 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9897 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9898 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9899 final Acceleration bx2 = new Acceleration(0.0,
9900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9901 calibrator.getInitialBiasXAsAcceleration(bx2);
9902 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9903 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9904 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9905 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9906 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9907 final Acceleration by2 = new Acceleration(0.0,
9908 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9909 calibrator.getInitialBiasYAsAcceleration(by2);
9910 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9911 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9912 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9913 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9914 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9915 final Acceleration bz2 = new Acceleration(0.0,
9916 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9917 calibrator.getInitialBiasZAsAcceleration(bz2);
9918 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9919 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9920 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9921 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9922 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9923 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9924 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9925 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9926 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9927 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9928 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9929 final double[] bias1 = calibrator.getInitialBias();
9930 assertArrayEquals(bias1, new double[3], 0.0);
9931 final double[] bias2 = new double[3];
9932 calibrator.getInitialBias(bias2);
9933 assertArrayEquals(bias1, bias2, 0.0);
9934 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9935 assertEquals(b1, new Matrix(3, 1));
9936 final Matrix b2 = new Matrix(3, 1);
9937 calibrator.getInitialBiasAsMatrix(b2);
9938 assertEquals(b1, b2);
9939 final Matrix ma1 = calibrator.getInitialMa();
9940 assertEquals(ma1, new Matrix(3, 3));
9941 final Matrix ma2 = new Matrix(3, 3);
9942 calibrator.getInitialMa(ma2);
9943 assertEquals(ma1, ma2);
9944 assertSame(calibrator.getEcefPosition(), ecefPosition);
9945 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9946 final NEDPosition nedPosition1 = new NEDPosition();
9947 assertTrue(calibrator.getNedPosition(nedPosition1));
9948 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9949 assertSame(calibrator.getMeasurements(), measurements);
9950 assertTrue(calibrator.isCommonAxisUsed());
9951 assertNull(calibrator.getListener());
9952 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9953 assertFalse(calibrator.isReady());
9954 assertFalse(calibrator.isRunning());
9955 assertNull(calibrator.getEstimatedBiases());
9956 assertFalse(calibrator.getEstimatedBiases(null));
9957 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9958 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9959 assertNull(calibrator.getEstimatedBiasFx());
9960 assertNull(calibrator.getEstimatedBiasFy());
9961 assertNull(calibrator.getEstimatedBiasFz());
9962 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9963 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9964 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9965 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9966 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9967 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9968 assertNull(calibrator.getEstimatedMa());
9969 assertNull(calibrator.getEstimatedSx());
9970 assertNull(calibrator.getEstimatedSy());
9971 assertNull(calibrator.getEstimatedSz());
9972 assertNull(calibrator.getEstimatedMxy());
9973 assertNull(calibrator.getEstimatedMxz());
9974 assertNull(calibrator.getEstimatedMyx());
9975 assertNull(calibrator.getEstimatedMyz());
9976 assertNull(calibrator.getEstimatedMzx());
9977 assertNull(calibrator.getEstimatedMzy());
9978 assertNull(calibrator.getEstimatedCovariance());
9979 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9980 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9981 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9982 assertNotNull(calibrator.getGroundTruthGravityNorm());
9983 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9984 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9985 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9986 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9987 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9988 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9989 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9990 }
9991
9992 @Test
9993 public void testConstructor86() throws WrongSizeException {
9994 final Collection<StandardDeviationBodyKinematics> measurements =
9995 Collections.emptyList();
9996 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9997 final double latitude = Math.toRadians(
9998 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9999 final double longitude = Math.toRadians(
10000 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10001 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10002 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10003 final NEDVelocity nedVelocity = new NEDVelocity();
10004 final ECEFPosition ecefPosition = new ECEFPosition();
10005 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10006 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10007 ecefPosition, ecefVelocity);
10008
10009 final KnownPositionAccelerometerCalibrator calibrator =
10010 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10011 true, this);
10012
10013
10014 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
10015 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
10016 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
10017 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10018 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
10019 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10020 final Acceleration bx2 = new Acceleration(0.0,
10021 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10022 calibrator.getInitialBiasXAsAcceleration(bx2);
10023 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
10024 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10025 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10026 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
10027 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10028 final Acceleration by2 = new Acceleration(0.0,
10029 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10030 calibrator.getInitialBiasYAsAcceleration(by2);
10031 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
10032 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10033 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10034 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
10035 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10036 final Acceleration bz2 = new Acceleration(0.0,
10037 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10038 calibrator.getInitialBiasZAsAcceleration(bz2);
10039 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
10040 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10041 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10042 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10043 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10044 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10045 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10046 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10047 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10048 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10049 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10050 final double[] bias1 = calibrator.getInitialBias();
10051 assertArrayEquals(bias1, new double[3], 0.0);
10052 final double[] bias2 = new double[3];
10053 calibrator.getInitialBias(bias2);
10054 assertArrayEquals(bias1, bias2, 0.0);
10055 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10056 assertEquals(b1, new Matrix(3, 1));
10057 final Matrix b2 = new Matrix(3, 1);
10058 calibrator.getInitialBiasAsMatrix(b2);
10059 assertEquals(b1, b2);
10060 final Matrix ma1 = calibrator.getInitialMa();
10061 assertEquals(ma1, new Matrix(3, 3));
10062 final Matrix ma2 = new Matrix(3, 3);
10063 calibrator.getInitialMa(ma2);
10064 assertEquals(ma1, ma2);
10065 assertSame(calibrator.getEcefPosition(), ecefPosition);
10066 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10067 final NEDPosition nedPosition1 = new NEDPosition();
10068 assertTrue(calibrator.getNedPosition(nedPosition1));
10069 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10070 assertSame(calibrator.getMeasurements(), measurements);
10071 assertTrue(calibrator.isCommonAxisUsed());
10072 assertSame(calibrator.getListener(), this);
10073 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10074 assertFalse(calibrator.isReady());
10075 assertFalse(calibrator.isRunning());
10076 assertNull(calibrator.getEstimatedBiases());
10077 assertFalse(calibrator.getEstimatedBiases(null));
10078 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10079 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10080 assertNull(calibrator.getEstimatedBiasFx());
10081 assertNull(calibrator.getEstimatedBiasFy());
10082 assertNull(calibrator.getEstimatedBiasFz());
10083 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10084 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10085 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10086 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10087 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10088 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10089 assertNull(calibrator.getEstimatedMa());
10090 assertNull(calibrator.getEstimatedSx());
10091 assertNull(calibrator.getEstimatedSy());
10092 assertNull(calibrator.getEstimatedSz());
10093 assertNull(calibrator.getEstimatedMxy());
10094 assertNull(calibrator.getEstimatedMxz());
10095 assertNull(calibrator.getEstimatedMyx());
10096 assertNull(calibrator.getEstimatedMyz());
10097 assertNull(calibrator.getEstimatedMzx());
10098 assertNull(calibrator.getEstimatedMzy());
10099 assertNull(calibrator.getEstimatedCovariance());
10100 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10101 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10102 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10103 assertNotNull(calibrator.getGroundTruthGravityNorm());
10104 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10105 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10106 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10107 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10108 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10109 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10110 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10111 }
10112
10113 @Test
10114 public void testConstructor87() throws WrongSizeException {
10115 final Matrix ba = generateBa();
10116 final double biasX = ba.getElementAtIndex(0);
10117 final double biasY = ba.getElementAtIndex(1);
10118 final double biasZ = ba.getElementAtIndex(2);
10119
10120 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10121 final double latitude = Math.toRadians(
10122 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10123 final double longitude = Math.toRadians(
10124 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10125 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10126 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10127 final NEDVelocity nedVelocity = new NEDVelocity();
10128 final ECEFPosition ecefPosition = new ECEFPosition();
10129 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10130 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10131 ecefPosition, ecefVelocity);
10132
10133 final KnownPositionAccelerometerCalibrator calibrator =
10134 new KnownPositionAccelerometerCalibrator(ecefPosition,
10135 biasX, biasY, biasZ);
10136
10137
10138 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10139 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10140 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10141 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10142 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10143 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10144 final Acceleration bx2 = new Acceleration(0.0,
10145 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10146 calibrator.getInitialBiasXAsAcceleration(bx2);
10147 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10148 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10149 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10150 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10151 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10152 final Acceleration by2 = new Acceleration(0.0,
10153 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10154 calibrator.getInitialBiasYAsAcceleration(by2);
10155 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10156 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10157 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10158 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10159 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10160 final Acceleration bz2 = new Acceleration(0.0,
10161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10162 calibrator.getInitialBiasZAsAcceleration(bz2);
10163 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10164 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10165 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10166 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10167 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10168 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10169 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10170 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10171 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10172 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10173 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10174 final double[] bias1 = calibrator.getInitialBias();
10175 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10176 final double[] bias2 = new double[3];
10177 calibrator.getInitialBias(bias2);
10178 assertArrayEquals(bias1, bias2, 0.0);
10179 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10180 assertEquals(b1, ba);
10181 final Matrix b2 = new Matrix(3, 1);
10182 calibrator.getInitialBiasAsMatrix(b2);
10183 assertEquals(b1, b2);
10184 final Matrix ma1 = calibrator.getInitialMa();
10185 assertEquals(ma1, new Matrix(3, 3));
10186 final Matrix ma2 = new Matrix(3, 3);
10187 calibrator.getInitialMa(ma2);
10188 assertEquals(ma1, ma2);
10189 assertSame(calibrator.getEcefPosition(), ecefPosition);
10190 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10191 final NEDPosition nedPosition1 = new NEDPosition();
10192 assertTrue(calibrator.getNedPosition(nedPosition1));
10193 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10194 assertNull(calibrator.getMeasurements());
10195 assertFalse(calibrator.isCommonAxisUsed());
10196 assertNull(calibrator.getListener());
10197 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10198 assertFalse(calibrator.isReady());
10199 assertFalse(calibrator.isRunning());
10200 assertNull(calibrator.getEstimatedBiases());
10201 assertFalse(calibrator.getEstimatedBiases(null));
10202 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10203 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10204 assertNull(calibrator.getEstimatedBiasFx());
10205 assertNull(calibrator.getEstimatedBiasFy());
10206 assertNull(calibrator.getEstimatedBiasFz());
10207 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10208 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10209 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10210 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10211 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10212 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10213 assertNull(calibrator.getEstimatedMa());
10214 assertNull(calibrator.getEstimatedSx());
10215 assertNull(calibrator.getEstimatedSy());
10216 assertNull(calibrator.getEstimatedSz());
10217 assertNull(calibrator.getEstimatedMxy());
10218 assertNull(calibrator.getEstimatedMxz());
10219 assertNull(calibrator.getEstimatedMyx());
10220 assertNull(calibrator.getEstimatedMyz());
10221 assertNull(calibrator.getEstimatedMzx());
10222 assertNull(calibrator.getEstimatedMzy());
10223 assertNull(calibrator.getEstimatedCovariance());
10224 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10225 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10226 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10227 assertNotNull(calibrator.getGroundTruthGravityNorm());
10228 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10229 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10230 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10231 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10232 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10233 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10234 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10235 }
10236
10237 @Test
10238 public void testConstructor88() throws WrongSizeException {
10239 final Matrix ba = generateBa();
10240 final double biasX = ba.getElementAtIndex(0);
10241 final double biasY = ba.getElementAtIndex(1);
10242 final double biasZ = ba.getElementAtIndex(2);
10243
10244 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10245 final double latitude = Math.toRadians(
10246 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10247 final double longitude = Math.toRadians(
10248 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10249 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10250 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10251 final NEDVelocity nedVelocity = new NEDVelocity();
10252 final ECEFPosition ecefPosition = new ECEFPosition();
10253 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10254 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10255 ecefPosition, ecefVelocity);
10256
10257 final KnownPositionAccelerometerCalibrator calibrator =
10258 new KnownPositionAccelerometerCalibrator(ecefPosition,
10259 biasX, biasY, biasZ, this);
10260
10261
10262 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10263 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10264 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10265 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10266 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10267 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10268 final Acceleration bx2 = new Acceleration(0.0,
10269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10270 calibrator.getInitialBiasXAsAcceleration(bx2);
10271 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10272 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10273 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10274 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10275 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10276 final Acceleration by2 = new Acceleration(0.0,
10277 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10278 calibrator.getInitialBiasYAsAcceleration(by2);
10279 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10280 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10281 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10282 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10283 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10284 final Acceleration bz2 = new Acceleration(0.0,
10285 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10286 calibrator.getInitialBiasZAsAcceleration(bz2);
10287 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10288 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10289 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10290 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10291 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10292 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10293 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10294 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10295 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10296 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10297 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10298 final double[] bias1 = calibrator.getInitialBias();
10299 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10300 final double[] bias2 = new double[3];
10301 calibrator.getInitialBias(bias2);
10302 assertArrayEquals(bias1, bias2, 0.0);
10303 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10304 assertEquals(b1, ba);
10305 final Matrix b2 = new Matrix(3, 1);
10306 calibrator.getInitialBiasAsMatrix(b2);
10307 assertEquals(b1, b2);
10308 final Matrix ma1 = calibrator.getInitialMa();
10309 assertEquals(ma1, new Matrix(3, 3));
10310 final Matrix ma2 = new Matrix(3, 3);
10311 calibrator.getInitialMa(ma2);
10312 assertEquals(ma1, ma2);
10313 assertSame(calibrator.getEcefPosition(), ecefPosition);
10314 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10315 final NEDPosition nedPosition1 = new NEDPosition();
10316 assertTrue(calibrator.getNedPosition(nedPosition1));
10317 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10318 assertNull(calibrator.getMeasurements());
10319 assertFalse(calibrator.isCommonAxisUsed());
10320 assertSame(calibrator.getListener(), this);
10321 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10322 assertFalse(calibrator.isReady());
10323 assertFalse(calibrator.isRunning());
10324 assertNull(calibrator.getEstimatedBiases());
10325 assertFalse(calibrator.getEstimatedBiases(null));
10326 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10327 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10328 assertNull(calibrator.getEstimatedBiasFx());
10329 assertNull(calibrator.getEstimatedBiasFy());
10330 assertNull(calibrator.getEstimatedBiasFz());
10331 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10332 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10333 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10334 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10335 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10336 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10337 assertNull(calibrator.getEstimatedMa());
10338 assertNull(calibrator.getEstimatedSx());
10339 assertNull(calibrator.getEstimatedSy());
10340 assertNull(calibrator.getEstimatedSz());
10341 assertNull(calibrator.getEstimatedMxy());
10342 assertNull(calibrator.getEstimatedMxz());
10343 assertNull(calibrator.getEstimatedMyx());
10344 assertNull(calibrator.getEstimatedMyz());
10345 assertNull(calibrator.getEstimatedMzx());
10346 assertNull(calibrator.getEstimatedMzy());
10347 assertNull(calibrator.getEstimatedCovariance());
10348 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10349 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10350 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10351 assertNotNull(calibrator.getGroundTruthGravityNorm());
10352 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10353 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10354 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10355 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10356 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10357 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10358 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10359 }
10360
10361 @Test
10362 public void testConstructor89() throws WrongSizeException {
10363 final Collection<StandardDeviationBodyKinematics> measurements =
10364 Collections.emptyList();
10365
10366 final Matrix ba = generateBa();
10367 final double biasX = ba.getElementAtIndex(0);
10368 final double biasY = ba.getElementAtIndex(1);
10369 final double biasZ = ba.getElementAtIndex(2);
10370
10371 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10372 final double latitude = Math.toRadians(
10373 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10374 final double longitude = Math.toRadians(
10375 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10376 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10377 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10378 final NEDVelocity nedVelocity = new NEDVelocity();
10379 final ECEFPosition ecefPosition = new ECEFPosition();
10380 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10381 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10382 ecefPosition, ecefVelocity);
10383
10384 final KnownPositionAccelerometerCalibrator calibrator =
10385 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10386 biasX, biasY, biasZ);
10387
10388
10389 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10390 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10391 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10392 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10393 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10394 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10395 final Acceleration bx2 = new Acceleration(0.0,
10396 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10397 calibrator.getInitialBiasXAsAcceleration(bx2);
10398 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10399 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10400 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10401 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10402 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10403 final Acceleration by2 = new Acceleration(0.0,
10404 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10405 calibrator.getInitialBiasYAsAcceleration(by2);
10406 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10407 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10408 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10409 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10410 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10411 final Acceleration bz2 = new Acceleration(0.0,
10412 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10413 calibrator.getInitialBiasZAsAcceleration(bz2);
10414 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10415 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10416 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10417 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10418 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10419 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10420 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10421 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10422 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10423 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10424 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10425 final double[] bias1 = calibrator.getInitialBias();
10426 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10427 final double[] bias2 = new double[3];
10428 calibrator.getInitialBias(bias2);
10429 assertArrayEquals(bias1, bias2, 0.0);
10430 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10431 assertEquals(b1, ba);
10432 final Matrix b2 = new Matrix(3, 1);
10433 calibrator.getInitialBiasAsMatrix(b2);
10434 assertEquals(b1, b2);
10435 final Matrix ma1 = calibrator.getInitialMa();
10436 assertEquals(ma1, new Matrix(3, 3));
10437 final Matrix ma2 = new Matrix(3, 3);
10438 calibrator.getInitialMa(ma2);
10439 assertEquals(ma1, ma2);
10440 assertSame(calibrator.getEcefPosition(), ecefPosition);
10441 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10442 final NEDPosition nedPosition1 = new NEDPosition();
10443 assertTrue(calibrator.getNedPosition(nedPosition1));
10444 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10445 assertSame(calibrator.getMeasurements(), measurements);
10446 assertFalse(calibrator.isCommonAxisUsed());
10447 assertNull(calibrator.getListener());
10448 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10449 assertFalse(calibrator.isReady());
10450 assertFalse(calibrator.isRunning());
10451 assertNull(calibrator.getEstimatedBiases());
10452 assertFalse(calibrator.getEstimatedBiases(null));
10453 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10454 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10455 assertNull(calibrator.getEstimatedBiasFx());
10456 assertNull(calibrator.getEstimatedBiasFy());
10457 assertNull(calibrator.getEstimatedBiasFz());
10458 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10459 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10460 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10461 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10462 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10463 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10464 assertNull(calibrator.getEstimatedMa());
10465 assertNull(calibrator.getEstimatedSx());
10466 assertNull(calibrator.getEstimatedSy());
10467 assertNull(calibrator.getEstimatedSz());
10468 assertNull(calibrator.getEstimatedMxy());
10469 assertNull(calibrator.getEstimatedMxz());
10470 assertNull(calibrator.getEstimatedMyx());
10471 assertNull(calibrator.getEstimatedMyz());
10472 assertNull(calibrator.getEstimatedMzx());
10473 assertNull(calibrator.getEstimatedMzy());
10474 assertNull(calibrator.getEstimatedCovariance());
10475 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10476 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10477 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10478 assertNotNull(calibrator.getGroundTruthGravityNorm());
10479 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10480 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10481 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10482 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10483 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10484 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10485 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10486 }
10487
10488 @Test
10489 public void testConstructor90() throws WrongSizeException {
10490 final Collection<StandardDeviationBodyKinematics> measurements =
10491 Collections.emptyList();
10492
10493 final Matrix ba = generateBa();
10494 final double biasX = ba.getElementAtIndex(0);
10495 final double biasY = ba.getElementAtIndex(1);
10496 final double biasZ = ba.getElementAtIndex(2);
10497
10498 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10499 final double latitude = Math.toRadians(
10500 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10501 final double longitude = Math.toRadians(
10502 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10503 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10504 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10505 final NEDVelocity nedVelocity = new NEDVelocity();
10506 final ECEFPosition ecefPosition = new ECEFPosition();
10507 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10508 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10509 ecefPosition, ecefVelocity);
10510
10511 final KnownPositionAccelerometerCalibrator calibrator =
10512 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10513 biasX, biasY, biasZ, this);
10514
10515
10516 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10517 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10518 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10519 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10520 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10521 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10522 final Acceleration bx2 = new Acceleration(0.0,
10523 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10524 calibrator.getInitialBiasXAsAcceleration(bx2);
10525 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10526 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10527 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10528 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10529 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10530 final Acceleration by2 = new Acceleration(0.0,
10531 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10532 calibrator.getInitialBiasYAsAcceleration(by2);
10533 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10534 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10535 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10536 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10537 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10538 final Acceleration bz2 = new Acceleration(0.0,
10539 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10540 calibrator.getInitialBiasZAsAcceleration(bz2);
10541 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10542 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10543 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10544 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10545 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10546 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10547 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10548 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10549 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10550 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10551 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10552 final double[] bias1 = calibrator.getInitialBias();
10553 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10554 final double[] bias2 = new double[3];
10555 calibrator.getInitialBias(bias2);
10556 assertArrayEquals(bias1, bias2, 0.0);
10557 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10558 assertEquals(b1, ba);
10559 final Matrix b2 = new Matrix(3, 1);
10560 calibrator.getInitialBiasAsMatrix(b2);
10561 assertEquals(b1, b2);
10562 final Matrix ma1 = calibrator.getInitialMa();
10563 assertEquals(ma1, new Matrix(3, 3));
10564 final Matrix ma2 = new Matrix(3, 3);
10565 calibrator.getInitialMa(ma2);
10566 assertEquals(ma1, ma2);
10567 assertSame(calibrator.getEcefPosition(), ecefPosition);
10568 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10569 final NEDPosition nedPosition1 = new NEDPosition();
10570 assertTrue(calibrator.getNedPosition(nedPosition1));
10571 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10572 assertSame(calibrator.getMeasurements(), measurements);
10573 assertFalse(calibrator.isCommonAxisUsed());
10574 assertSame(calibrator.getListener(), this);
10575 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10576 assertFalse(calibrator.isReady());
10577 assertFalse(calibrator.isRunning());
10578 assertNull(calibrator.getEstimatedBiases());
10579 assertFalse(calibrator.getEstimatedBiases(null));
10580 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10581 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10582 assertNull(calibrator.getEstimatedBiasFx());
10583 assertNull(calibrator.getEstimatedBiasFy());
10584 assertNull(calibrator.getEstimatedBiasFz());
10585 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10586 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10587 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10588 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10589 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10590 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10591 assertNull(calibrator.getEstimatedMa());
10592 assertNull(calibrator.getEstimatedSx());
10593 assertNull(calibrator.getEstimatedSy());
10594 assertNull(calibrator.getEstimatedSz());
10595 assertNull(calibrator.getEstimatedMxy());
10596 assertNull(calibrator.getEstimatedMxz());
10597 assertNull(calibrator.getEstimatedMyx());
10598 assertNull(calibrator.getEstimatedMyz());
10599 assertNull(calibrator.getEstimatedMzx());
10600 assertNull(calibrator.getEstimatedMzy());
10601 assertNull(calibrator.getEstimatedCovariance());
10602 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10603 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10604 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10605 assertNotNull(calibrator.getGroundTruthGravityNorm());
10606 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10607 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10608 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10609 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10610 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10611 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10612 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10613 }
10614
10615 @Test
10616 public void testConstructor91() throws WrongSizeException {
10617 final Matrix ba = generateBa();
10618 final double biasX = ba.getElementAtIndex(0);
10619 final double biasY = ba.getElementAtIndex(1);
10620 final double biasZ = ba.getElementAtIndex(2);
10621
10622 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10623 final double latitude = Math.toRadians(
10624 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10625 final double longitude = Math.toRadians(
10626 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10627 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10628 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10629 final NEDVelocity nedVelocity = new NEDVelocity();
10630 final ECEFPosition ecefPosition = new ECEFPosition();
10631 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10632 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10633 ecefPosition, ecefVelocity);
10634
10635 final KnownPositionAccelerometerCalibrator calibrator =
10636 new KnownPositionAccelerometerCalibrator(ecefPosition,
10637 true, biasX, biasY, biasZ);
10638
10639
10640 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10641 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10642 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10643 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10644 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10645 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10646 final Acceleration bx2 = new Acceleration(0.0,
10647 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10648 calibrator.getInitialBiasXAsAcceleration(bx2);
10649 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10650 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10651 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10652 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10653 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10654 final Acceleration by2 = new Acceleration(0.0,
10655 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10656 calibrator.getInitialBiasYAsAcceleration(by2);
10657 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10658 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10659 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10660 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10661 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10662 final Acceleration bz2 = new Acceleration(0.0,
10663 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10664 calibrator.getInitialBiasZAsAcceleration(bz2);
10665 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10666 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10667 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10668 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10669 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10670 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10671 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10672 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10673 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10674 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10675 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10676 final double[] bias1 = calibrator.getInitialBias();
10677 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10678 final double[] bias2 = new double[3];
10679 calibrator.getInitialBias(bias2);
10680 assertArrayEquals(bias1, bias2, 0.0);
10681 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10682 assertEquals(b1, ba);
10683 final Matrix b2 = new Matrix(3, 1);
10684 calibrator.getInitialBiasAsMatrix(b2);
10685 assertEquals(b1, b2);
10686 final Matrix ma1 = calibrator.getInitialMa();
10687 assertEquals(ma1, new Matrix(3, 3));
10688 final Matrix ma2 = new Matrix(3, 3);
10689 calibrator.getInitialMa(ma2);
10690 assertEquals(ma1, ma2);
10691 assertSame(calibrator.getEcefPosition(), ecefPosition);
10692 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10693 final NEDPosition nedPosition1 = new NEDPosition();
10694 assertTrue(calibrator.getNedPosition(nedPosition1));
10695 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10696 assertNull(calibrator.getMeasurements());
10697 assertTrue(calibrator.isCommonAxisUsed());
10698 assertNull(calibrator.getListener());
10699 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10700 assertFalse(calibrator.isReady());
10701 assertFalse(calibrator.isRunning());
10702 assertNull(calibrator.getEstimatedBiases());
10703 assertFalse(calibrator.getEstimatedBiases(null));
10704 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10705 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10706 assertNull(calibrator.getEstimatedBiasFx());
10707 assertNull(calibrator.getEstimatedBiasFy());
10708 assertNull(calibrator.getEstimatedBiasFz());
10709 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10710 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10711 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10712 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10713 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10714 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10715 assertNull(calibrator.getEstimatedMa());
10716 assertNull(calibrator.getEstimatedSx());
10717 assertNull(calibrator.getEstimatedSy());
10718 assertNull(calibrator.getEstimatedSz());
10719 assertNull(calibrator.getEstimatedMxy());
10720 assertNull(calibrator.getEstimatedMxz());
10721 assertNull(calibrator.getEstimatedMyx());
10722 assertNull(calibrator.getEstimatedMyz());
10723 assertNull(calibrator.getEstimatedMzx());
10724 assertNull(calibrator.getEstimatedMzy());
10725 assertNull(calibrator.getEstimatedCovariance());
10726 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10727 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10728 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10729 assertNotNull(calibrator.getGroundTruthGravityNorm());
10730 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10731 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10732 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10733 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10734 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10735 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10736 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10737 }
10738
10739 @Test
10740 public void testConstructor92() throws WrongSizeException {
10741 final Matrix ba = generateBa();
10742 final double biasX = ba.getElementAtIndex(0);
10743 final double biasY = ba.getElementAtIndex(1);
10744 final double biasZ = ba.getElementAtIndex(2);
10745
10746 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10747 final double latitude = Math.toRadians(
10748 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10749 final double longitude = Math.toRadians(
10750 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10751 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10752 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10753 final NEDVelocity nedVelocity = new NEDVelocity();
10754 final ECEFPosition ecefPosition = new ECEFPosition();
10755 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10756 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10757 ecefPosition, ecefVelocity);
10758
10759 final KnownPositionAccelerometerCalibrator calibrator =
10760 new KnownPositionAccelerometerCalibrator(ecefPosition,
10761 true, biasX, biasY, biasZ, this);
10762
10763
10764 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10765 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10766 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10767 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10768 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10769 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10770 final Acceleration bx2 = new Acceleration(0.0,
10771 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10772 calibrator.getInitialBiasXAsAcceleration(bx2);
10773 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10774 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10775 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10776 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10777 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10778 final Acceleration by2 = new Acceleration(0.0,
10779 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10780 calibrator.getInitialBiasYAsAcceleration(by2);
10781 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10782 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10783 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10784 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10785 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10786 final Acceleration bz2 = new Acceleration(0.0,
10787 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10788 calibrator.getInitialBiasZAsAcceleration(bz2);
10789 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10790 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10791 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10792 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10793 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10794 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10795 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10796 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10797 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10798 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10799 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10800 final double[] bias1 = calibrator.getInitialBias();
10801 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10802 final double[] bias2 = new double[3];
10803 calibrator.getInitialBias(bias2);
10804 assertArrayEquals(bias1, bias2, 0.0);
10805 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10806 assertEquals(b1, ba);
10807 final Matrix b2 = new Matrix(3, 1);
10808 calibrator.getInitialBiasAsMatrix(b2);
10809 assertEquals(b1, b2);
10810 final Matrix ma1 = calibrator.getInitialMa();
10811 assertEquals(ma1, new Matrix(3, 3));
10812 final Matrix ma2 = new Matrix(3, 3);
10813 calibrator.getInitialMa(ma2);
10814 assertEquals(ma1, ma2);
10815 assertSame(calibrator.getEcefPosition(), ecefPosition);
10816 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10817 final NEDPosition nedPosition1 = new NEDPosition();
10818 assertTrue(calibrator.getNedPosition(nedPosition1));
10819 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10820 assertNull(calibrator.getMeasurements());
10821 assertTrue(calibrator.isCommonAxisUsed());
10822 assertSame(calibrator.getListener(), this);
10823 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10824 assertFalse(calibrator.isReady());
10825 assertFalse(calibrator.isRunning());
10826 assertNull(calibrator.getEstimatedBiases());
10827 assertFalse(calibrator.getEstimatedBiases(null));
10828 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10829 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10830 assertNull(calibrator.getEstimatedBiasFx());
10831 assertNull(calibrator.getEstimatedBiasFy());
10832 assertNull(calibrator.getEstimatedBiasFz());
10833 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10834 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10835 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10836 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10837 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10838 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10839 assertNull(calibrator.getEstimatedMa());
10840 assertNull(calibrator.getEstimatedSx());
10841 assertNull(calibrator.getEstimatedSy());
10842 assertNull(calibrator.getEstimatedSz());
10843 assertNull(calibrator.getEstimatedMxy());
10844 assertNull(calibrator.getEstimatedMxz());
10845 assertNull(calibrator.getEstimatedMyx());
10846 assertNull(calibrator.getEstimatedMyz());
10847 assertNull(calibrator.getEstimatedMzx());
10848 assertNull(calibrator.getEstimatedMzy());
10849 assertNull(calibrator.getEstimatedCovariance());
10850 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10851 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10852 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10853 assertNotNull(calibrator.getGroundTruthGravityNorm());
10854 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10855 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10856 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10857 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10858 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10859 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10860 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10861 }
10862
10863 @Test
10864 public void testConstructor93() throws WrongSizeException {
10865 final Collection<StandardDeviationBodyKinematics> measurements =
10866 Collections.emptyList();
10867
10868 final Matrix ba = generateBa();
10869 final double biasX = ba.getElementAtIndex(0);
10870 final double biasY = ba.getElementAtIndex(1);
10871 final double biasZ = ba.getElementAtIndex(2);
10872
10873 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10874 final double latitude = Math.toRadians(
10875 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10876 final double longitude = Math.toRadians(
10877 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10878 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10879 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10880 final NEDVelocity nedVelocity = new NEDVelocity();
10881 final ECEFPosition ecefPosition = new ECEFPosition();
10882 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10883 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10884 ecefPosition, ecefVelocity);
10885
10886 final KnownPositionAccelerometerCalibrator calibrator =
10887 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10888 true, biasX, biasY, biasZ);
10889
10890
10891 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10892 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10893 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10894 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10895 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10896 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10897 final Acceleration bx2 = new Acceleration(0.0,
10898 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10899 calibrator.getInitialBiasXAsAcceleration(bx2);
10900 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10901 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10902 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10903 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10904 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10905 final Acceleration by2 = new Acceleration(0.0,
10906 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10907 calibrator.getInitialBiasYAsAcceleration(by2);
10908 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10909 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10910 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10911 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10912 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10913 final Acceleration bz2 = new Acceleration(0.0,
10914 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10915 calibrator.getInitialBiasZAsAcceleration(bz2);
10916 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10917 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10918 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10919 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10920 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10921 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10922 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10923 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10924 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10925 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10926 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10927 final double[] bias1 = calibrator.getInitialBias();
10928 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10929 final double[] bias2 = new double[3];
10930 calibrator.getInitialBias(bias2);
10931 assertArrayEquals(bias1, bias2, 0.0);
10932 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10933 assertEquals(b1, ba);
10934 final Matrix b2 = new Matrix(3, 1);
10935 calibrator.getInitialBiasAsMatrix(b2);
10936 assertEquals(b1, b2);
10937 final Matrix ma1 = calibrator.getInitialMa();
10938 assertEquals(ma1, new Matrix(3, 3));
10939 final Matrix ma2 = new Matrix(3, 3);
10940 calibrator.getInitialMa(ma2);
10941 assertEquals(ma1, ma2);
10942 assertSame(calibrator.getEcefPosition(), ecefPosition);
10943 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10944 final NEDPosition nedPosition1 = new NEDPosition();
10945 assertTrue(calibrator.getNedPosition(nedPosition1));
10946 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10947 assertSame(calibrator.getMeasurements(), measurements);
10948 assertTrue(calibrator.isCommonAxisUsed());
10949 assertNull(calibrator.getListener());
10950 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10951 assertFalse(calibrator.isReady());
10952 assertFalse(calibrator.isRunning());
10953 assertNull(calibrator.getEstimatedBiases());
10954 assertFalse(calibrator.getEstimatedBiases(null));
10955 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10956 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10957 assertNull(calibrator.getEstimatedBiasFx());
10958 assertNull(calibrator.getEstimatedBiasFy());
10959 assertNull(calibrator.getEstimatedBiasFz());
10960 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10961 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10962 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10963 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10964 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10965 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10966 assertNull(calibrator.getEstimatedMa());
10967 assertNull(calibrator.getEstimatedSx());
10968 assertNull(calibrator.getEstimatedSy());
10969 assertNull(calibrator.getEstimatedSz());
10970 assertNull(calibrator.getEstimatedMxy());
10971 assertNull(calibrator.getEstimatedMxz());
10972 assertNull(calibrator.getEstimatedMyx());
10973 assertNull(calibrator.getEstimatedMyz());
10974 assertNull(calibrator.getEstimatedMzx());
10975 assertNull(calibrator.getEstimatedMzy());
10976 assertNull(calibrator.getEstimatedCovariance());
10977 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10978 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10979 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10980 assertNotNull(calibrator.getGroundTruthGravityNorm());
10981 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10982 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10983 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10984 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10985 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10986 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10987 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10988 }
10989
10990 @Test
10991 public void testConstructor94() throws WrongSizeException {
10992 final Collection<StandardDeviationBodyKinematics> measurements =
10993 Collections.emptyList();
10994
10995 final Matrix ba = generateBa();
10996 final double biasX = ba.getElementAtIndex(0);
10997 final double biasY = ba.getElementAtIndex(1);
10998 final double biasZ = ba.getElementAtIndex(2);
10999
11000 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11001 final double latitude = Math.toRadians(
11002 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11003 final double longitude = Math.toRadians(
11004 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11005 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11006 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11007 final NEDVelocity nedVelocity = new NEDVelocity();
11008 final ECEFPosition ecefPosition = new ECEFPosition();
11009 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11010 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11011 ecefPosition, ecefVelocity);
11012
11013 final KnownPositionAccelerometerCalibrator calibrator =
11014 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11015 true, biasX, biasY, biasZ, this);
11016
11017
11018 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11019 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11020 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11021 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11022 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11023 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11024 final Acceleration bx2 = new Acceleration(0.0,
11025 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11026 calibrator.getInitialBiasXAsAcceleration(bx2);
11027 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11028 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11029 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11030 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11031 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11032 final Acceleration by2 = new Acceleration(0.0,
11033 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11034 calibrator.getInitialBiasYAsAcceleration(by2);
11035 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11036 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11037 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11038 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11039 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11040 final Acceleration bz2 = new Acceleration(0.0,
11041 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11042 calibrator.getInitialBiasZAsAcceleration(bz2);
11043 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11044 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11045 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11046 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11047 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11048 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11049 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11050 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11051 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11052 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11053 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11054 final double[] bias1 = calibrator.getInitialBias();
11055 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11056 final double[] bias2 = new double[3];
11057 calibrator.getInitialBias(bias2);
11058 assertArrayEquals(bias1, bias2, 0.0);
11059 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11060 assertEquals(b1, ba);
11061 final Matrix b2 = new Matrix(3, 1);
11062 calibrator.getInitialBiasAsMatrix(b2);
11063 assertEquals(b1, b2);
11064 final Matrix ma1 = calibrator.getInitialMa();
11065 assertEquals(ma1, new Matrix(3, 3));
11066 final Matrix ma2 = new Matrix(3, 3);
11067 calibrator.getInitialMa(ma2);
11068 assertEquals(ma1, ma2);
11069 assertSame(calibrator.getEcefPosition(), ecefPosition);
11070 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11071 final NEDPosition nedPosition1 = new NEDPosition();
11072 assertTrue(calibrator.getNedPosition(nedPosition1));
11073 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11074 assertSame(calibrator.getMeasurements(), measurements);
11075 assertTrue(calibrator.isCommonAxisUsed());
11076 assertSame(calibrator.getListener(), this);
11077 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11078 assertFalse(calibrator.isReady());
11079 assertFalse(calibrator.isRunning());
11080 assertNull(calibrator.getEstimatedBiases());
11081 assertFalse(calibrator.getEstimatedBiases(null));
11082 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11083 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11084 assertNull(calibrator.getEstimatedBiasFx());
11085 assertNull(calibrator.getEstimatedBiasFy());
11086 assertNull(calibrator.getEstimatedBiasFz());
11087 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11088 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11089 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11090 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11091 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11092 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11093 assertNull(calibrator.getEstimatedMa());
11094 assertNull(calibrator.getEstimatedSx());
11095 assertNull(calibrator.getEstimatedSy());
11096 assertNull(calibrator.getEstimatedSz());
11097 assertNull(calibrator.getEstimatedMxy());
11098 assertNull(calibrator.getEstimatedMxz());
11099 assertNull(calibrator.getEstimatedMyx());
11100 assertNull(calibrator.getEstimatedMyz());
11101 assertNull(calibrator.getEstimatedMzx());
11102 assertNull(calibrator.getEstimatedMzy());
11103 assertNull(calibrator.getEstimatedCovariance());
11104 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11105 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11106 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11107 assertNotNull(calibrator.getGroundTruthGravityNorm());
11108 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11109 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11110 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11111 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11112 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11113 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11114 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11115 }
11116
11117 @Test
11118 public void testConstructor95() throws WrongSizeException {
11119 final Matrix ba = generateBa();
11120 final double biasX = ba.getElementAtIndex(0);
11121 final double biasY = ba.getElementAtIndex(1);
11122 final double biasZ = ba.getElementAtIndex(2);
11123
11124 final Acceleration bx = new Acceleration(biasX,
11125 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11126 final Acceleration by = new Acceleration(biasY,
11127 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11128 final Acceleration bz = new Acceleration(biasZ,
11129 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11130
11131 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11132 final double latitude = Math.toRadians(
11133 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11134 final double longitude = Math.toRadians(
11135 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11136 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11137 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11138 final NEDVelocity nedVelocity = new NEDVelocity();
11139 final ECEFPosition ecefPosition = new ECEFPosition();
11140 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11141 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11142 ecefPosition, ecefVelocity);
11143
11144 final KnownPositionAccelerometerCalibrator calibrator =
11145 new KnownPositionAccelerometerCalibrator(ecefPosition,
11146 bx, by, bz);
11147
11148
11149 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11150 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11151 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11152 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11153 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11154 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11155 final Acceleration bx2 = new Acceleration(0.0,
11156 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11157 calibrator.getInitialBiasXAsAcceleration(bx2);
11158 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11159 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11160 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11161 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11162 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11163 final Acceleration by2 = new Acceleration(0.0,
11164 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11165 calibrator.getInitialBiasYAsAcceleration(by2);
11166 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11167 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11168 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11169 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11170 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11171 final Acceleration bz2 = new Acceleration(0.0,
11172 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11173 calibrator.getInitialBiasZAsAcceleration(bz2);
11174 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11175 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11176 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11177 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11178 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11179 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11180 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11181 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11182 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11183 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11184 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11185 final double[] bias1 = calibrator.getInitialBias();
11186 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11187 final double[] bias2 = new double[3];
11188 calibrator.getInitialBias(bias2);
11189 assertArrayEquals(bias1, bias2, 0.0);
11190 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11191 assertEquals(b1, ba);
11192 final Matrix b2 = new Matrix(3, 1);
11193 calibrator.getInitialBiasAsMatrix(b2);
11194 assertEquals(b1, b2);
11195 final Matrix ma1 = calibrator.getInitialMa();
11196 assertEquals(ma1, new Matrix(3, 3));
11197 final Matrix ma2 = new Matrix(3, 3);
11198 calibrator.getInitialMa(ma2);
11199 assertEquals(ma1, ma2);
11200 assertSame(calibrator.getEcefPosition(), ecefPosition);
11201 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11202 final NEDPosition nedPosition1 = new NEDPosition();
11203 assertTrue(calibrator.getNedPosition(nedPosition1));
11204 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11205 assertNull(calibrator.getMeasurements());
11206 assertFalse(calibrator.isCommonAxisUsed());
11207 assertNull(calibrator.getListener());
11208 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11209 assertFalse(calibrator.isReady());
11210 assertFalse(calibrator.isRunning());
11211 assertNull(calibrator.getEstimatedBiases());
11212 assertFalse(calibrator.getEstimatedBiases(null));
11213 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11214 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11215 assertNull(calibrator.getEstimatedBiasFx());
11216 assertNull(calibrator.getEstimatedBiasFy());
11217 assertNull(calibrator.getEstimatedBiasFz());
11218 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11219 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11220 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11221 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11222 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11223 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11224 assertNull(calibrator.getEstimatedMa());
11225 assertNull(calibrator.getEstimatedSx());
11226 assertNull(calibrator.getEstimatedSy());
11227 assertNull(calibrator.getEstimatedSz());
11228 assertNull(calibrator.getEstimatedMxy());
11229 assertNull(calibrator.getEstimatedMxz());
11230 assertNull(calibrator.getEstimatedMyx());
11231 assertNull(calibrator.getEstimatedMyz());
11232 assertNull(calibrator.getEstimatedMzx());
11233 assertNull(calibrator.getEstimatedMzy());
11234 assertNull(calibrator.getEstimatedCovariance());
11235 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11236 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11237 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11238 assertNotNull(calibrator.getGroundTruthGravityNorm());
11239 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11240 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11241 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11242 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11243 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11244 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11245 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11246 }
11247
11248 @Test
11249 public void testConstructor96() throws WrongSizeException {
11250 final Matrix ba = generateBa();
11251 final double biasX = ba.getElementAtIndex(0);
11252 final double biasY = ba.getElementAtIndex(1);
11253 final double biasZ = ba.getElementAtIndex(2);
11254
11255 final Acceleration bx = new Acceleration(biasX,
11256 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11257 final Acceleration by = new Acceleration(biasY,
11258 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11259 final Acceleration bz = new Acceleration(biasZ,
11260 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11261
11262 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11263 final double latitude = Math.toRadians(
11264 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11265 final double longitude = Math.toRadians(
11266 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11267 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11268 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11269 final NEDVelocity nedVelocity = new NEDVelocity();
11270 final ECEFPosition ecefPosition = new ECEFPosition();
11271 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11272 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11273 ecefPosition, ecefVelocity);
11274
11275 final KnownPositionAccelerometerCalibrator calibrator =
11276 new KnownPositionAccelerometerCalibrator(ecefPosition,
11277 bx, by, bz, this);
11278
11279
11280 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11281 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11282 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11283 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11284 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11285 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11286 final Acceleration bx2 = new Acceleration(0.0,
11287 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11288 calibrator.getInitialBiasXAsAcceleration(bx2);
11289 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11290 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11291 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11292 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11293 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11294 final Acceleration by2 = new Acceleration(0.0,
11295 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11296 calibrator.getInitialBiasYAsAcceleration(by2);
11297 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11298 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11299 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11300 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11301 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11302 final Acceleration bz2 = new Acceleration(0.0,
11303 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11304 calibrator.getInitialBiasZAsAcceleration(bz2);
11305 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11306 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11307 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11308 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11309 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11310 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11311 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11312 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11313 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11314 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11315 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11316 final double[] bias1 = calibrator.getInitialBias();
11317 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11318 final double[] bias2 = new double[3];
11319 calibrator.getInitialBias(bias2);
11320 assertArrayEquals(bias1, bias2, 0.0);
11321 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11322 assertEquals(b1, ba);
11323 final Matrix b2 = new Matrix(3, 1);
11324 calibrator.getInitialBiasAsMatrix(b2);
11325 assertEquals(b1, b2);
11326 final Matrix ma1 = calibrator.getInitialMa();
11327 assertEquals(ma1, new Matrix(3, 3));
11328 final Matrix ma2 = new Matrix(3, 3);
11329 calibrator.getInitialMa(ma2);
11330 assertEquals(ma1, ma2);
11331 assertSame(calibrator.getEcefPosition(), ecefPosition);
11332 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11333 final NEDPosition nedPosition1 = new NEDPosition();
11334 assertTrue(calibrator.getNedPosition(nedPosition1));
11335 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11336 assertNull(calibrator.getMeasurements());
11337 assertFalse(calibrator.isCommonAxisUsed());
11338 assertSame(calibrator.getListener(), this);
11339 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11340 assertFalse(calibrator.isReady());
11341 assertFalse(calibrator.isRunning());
11342 assertNull(calibrator.getEstimatedBiases());
11343 assertFalse(calibrator.getEstimatedBiases(null));
11344 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11345 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11346 assertNull(calibrator.getEstimatedBiasFx());
11347 assertNull(calibrator.getEstimatedBiasFy());
11348 assertNull(calibrator.getEstimatedBiasFz());
11349 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11350 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11351 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11352 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11353 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11354 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11355 assertNull(calibrator.getEstimatedMa());
11356 assertNull(calibrator.getEstimatedSx());
11357 assertNull(calibrator.getEstimatedSy());
11358 assertNull(calibrator.getEstimatedSz());
11359 assertNull(calibrator.getEstimatedMxy());
11360 assertNull(calibrator.getEstimatedMxz());
11361 assertNull(calibrator.getEstimatedMyx());
11362 assertNull(calibrator.getEstimatedMyz());
11363 assertNull(calibrator.getEstimatedMzx());
11364 assertNull(calibrator.getEstimatedMzy());
11365 assertNull(calibrator.getEstimatedCovariance());
11366 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11367 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11368 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11369 assertNotNull(calibrator.getGroundTruthGravityNorm());
11370 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11371 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11372 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11373 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11374 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11375 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11376 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11377 }
11378
11379 @Test
11380 public void testConstructor97() throws WrongSizeException {
11381 final Collection<StandardDeviationBodyKinematics> measurements =
11382 Collections.emptyList();
11383
11384 final Matrix ba = generateBa();
11385 final double biasX = ba.getElementAtIndex(0);
11386 final double biasY = ba.getElementAtIndex(1);
11387 final double biasZ = ba.getElementAtIndex(2);
11388
11389 final Acceleration bx = new Acceleration(biasX,
11390 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11391 final Acceleration by = new Acceleration(biasY,
11392 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11393 final Acceleration bz = new Acceleration(biasZ,
11394 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11395
11396 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11397 final double latitude = Math.toRadians(
11398 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11399 final double longitude = Math.toRadians(
11400 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11401 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11402 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11403 final NEDVelocity nedVelocity = new NEDVelocity();
11404 final ECEFPosition ecefPosition = new ECEFPosition();
11405 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11406 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11407 ecefPosition, ecefVelocity);
11408
11409 final KnownPositionAccelerometerCalibrator calibrator =
11410 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11411 bx, by, bz);
11412
11413
11414 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11415 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11416 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11417 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11418 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11419 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11420 final Acceleration bx2 = new Acceleration(0.0,
11421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11422 calibrator.getInitialBiasXAsAcceleration(bx2);
11423 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11424 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11425 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11426 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11427 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11428 final Acceleration by2 = new Acceleration(0.0,
11429 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11430 calibrator.getInitialBiasYAsAcceleration(by2);
11431 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11432 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11433 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11434 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11435 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11436 final Acceleration bz2 = new Acceleration(0.0,
11437 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11438 calibrator.getInitialBiasZAsAcceleration(bz2);
11439 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11440 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11441 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11442 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11443 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11444 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11445 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11446 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11447 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11448 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11449 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11450 final double[] bias1 = calibrator.getInitialBias();
11451 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11452 final double[] bias2 = new double[3];
11453 calibrator.getInitialBias(bias2);
11454 assertArrayEquals(bias1, bias2, 0.0);
11455 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11456 assertEquals(b1, ba);
11457 final Matrix b2 = new Matrix(3, 1);
11458 calibrator.getInitialBiasAsMatrix(b2);
11459 assertEquals(b1, b2);
11460 final Matrix ma1 = calibrator.getInitialMa();
11461 assertEquals(ma1, new Matrix(3, 3));
11462 final Matrix ma2 = new Matrix(3, 3);
11463 calibrator.getInitialMa(ma2);
11464 assertEquals(ma1, ma2);
11465 assertSame(calibrator.getEcefPosition(), ecefPosition);
11466 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11467 final NEDPosition nedPosition1 = new NEDPosition();
11468 assertTrue(calibrator.getNedPosition(nedPosition1));
11469 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11470 assertSame(calibrator.getMeasurements(), measurements);
11471 assertFalse(calibrator.isCommonAxisUsed());
11472 assertNull(calibrator.getListener());
11473 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11474 assertFalse(calibrator.isReady());
11475 assertFalse(calibrator.isRunning());
11476 assertNull(calibrator.getEstimatedBiases());
11477 assertFalse(calibrator.getEstimatedBiases(null));
11478 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11479 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11480 assertNull(calibrator.getEstimatedBiasFx());
11481 assertNull(calibrator.getEstimatedBiasFy());
11482 assertNull(calibrator.getEstimatedBiasFz());
11483 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11484 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11485 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11486 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11487 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11488 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11489 assertNull(calibrator.getEstimatedMa());
11490 assertNull(calibrator.getEstimatedSx());
11491 assertNull(calibrator.getEstimatedSy());
11492 assertNull(calibrator.getEstimatedSz());
11493 assertNull(calibrator.getEstimatedMxy());
11494 assertNull(calibrator.getEstimatedMxz());
11495 assertNull(calibrator.getEstimatedMyx());
11496 assertNull(calibrator.getEstimatedMyz());
11497 assertNull(calibrator.getEstimatedMzx());
11498 assertNull(calibrator.getEstimatedMzy());
11499 assertNull(calibrator.getEstimatedCovariance());
11500 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11501 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11502 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11503 assertNotNull(calibrator.getGroundTruthGravityNorm());
11504 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11505 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11506 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11507 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11508 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11509 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11510 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11511 }
11512
11513 @Test
11514 public void testConstructor98() throws WrongSizeException {
11515 final Collection<StandardDeviationBodyKinematics> measurements =
11516 Collections.emptyList();
11517
11518 final Matrix ba = generateBa();
11519 final double biasX = ba.getElementAtIndex(0);
11520 final double biasY = ba.getElementAtIndex(1);
11521 final double biasZ = ba.getElementAtIndex(2);
11522
11523 final Acceleration bx = new Acceleration(biasX,
11524 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11525 final Acceleration by = new Acceleration(biasY,
11526 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11527 final Acceleration bz = new Acceleration(biasZ,
11528 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11529
11530 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11531 final double latitude = Math.toRadians(
11532 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11533 final double longitude = Math.toRadians(
11534 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11535 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11536 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11537 final NEDVelocity nedVelocity = new NEDVelocity();
11538 final ECEFPosition ecefPosition = new ECEFPosition();
11539 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11540 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11541 ecefPosition, ecefVelocity);
11542
11543 final KnownPositionAccelerometerCalibrator calibrator =
11544 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11545 bx, by, bz, this);
11546
11547
11548 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11549 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11550 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11551 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11552 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11553 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11554 final Acceleration bx2 = new Acceleration(0.0,
11555 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11556 calibrator.getInitialBiasXAsAcceleration(bx2);
11557 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11558 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11559 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11560 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11561 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11562 final Acceleration by2 = new Acceleration(0.0,
11563 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11564 calibrator.getInitialBiasYAsAcceleration(by2);
11565 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11566 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11567 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11568 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11569 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11570 final Acceleration bz2 = new Acceleration(0.0,
11571 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11572 calibrator.getInitialBiasZAsAcceleration(bz2);
11573 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11574 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11575 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11576 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11577 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11578 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11579 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11580 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11581 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11582 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11583 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11584 final double[] bias1 = calibrator.getInitialBias();
11585 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11586 final double[] bias2 = new double[3];
11587 calibrator.getInitialBias(bias2);
11588 assertArrayEquals(bias1, bias2, 0.0);
11589 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11590 assertEquals(b1, ba);
11591 final Matrix b2 = new Matrix(3, 1);
11592 calibrator.getInitialBiasAsMatrix(b2);
11593 assertEquals(b1, b2);
11594 final Matrix ma1 = calibrator.getInitialMa();
11595 assertEquals(ma1, new Matrix(3, 3));
11596 final Matrix ma2 = new Matrix(3, 3);
11597 calibrator.getInitialMa(ma2);
11598 assertEquals(ma1, ma2);
11599 assertSame(calibrator.getEcefPosition(), ecefPosition);
11600 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11601 final NEDPosition nedPosition1 = new NEDPosition();
11602 assertTrue(calibrator.getNedPosition(nedPosition1));
11603 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11604 assertSame(calibrator.getMeasurements(), measurements);
11605 assertFalse(calibrator.isCommonAxisUsed());
11606 assertSame(calibrator.getListener(), this);
11607 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11608 assertFalse(calibrator.isReady());
11609 assertFalse(calibrator.isRunning());
11610 assertNull(calibrator.getEstimatedBiases());
11611 assertFalse(calibrator.getEstimatedBiases(null));
11612 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11613 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11614 assertNull(calibrator.getEstimatedBiasFx());
11615 assertNull(calibrator.getEstimatedBiasFy());
11616 assertNull(calibrator.getEstimatedBiasFz());
11617 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11618 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11619 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11620 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11621 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11622 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11623 assertNull(calibrator.getEstimatedMa());
11624 assertNull(calibrator.getEstimatedSx());
11625 assertNull(calibrator.getEstimatedSy());
11626 assertNull(calibrator.getEstimatedSz());
11627 assertNull(calibrator.getEstimatedMxy());
11628 assertNull(calibrator.getEstimatedMxz());
11629 assertNull(calibrator.getEstimatedMyx());
11630 assertNull(calibrator.getEstimatedMyz());
11631 assertNull(calibrator.getEstimatedMzx());
11632 assertNull(calibrator.getEstimatedMzy());
11633 assertNull(calibrator.getEstimatedCovariance());
11634 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11635 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11636 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11637 assertNotNull(calibrator.getGroundTruthGravityNorm());
11638 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11639 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11640 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11641 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11642 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11643 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11644 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11645 }
11646
11647 @Test
11648 public void testConstructor99() throws WrongSizeException {
11649 final Matrix ba = generateBa();
11650 final double biasX = ba.getElementAtIndex(0);
11651 final double biasY = ba.getElementAtIndex(1);
11652 final double biasZ = ba.getElementAtIndex(2);
11653
11654 final Acceleration bx = new Acceleration(biasX,
11655 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11656 final Acceleration by = new Acceleration(biasY,
11657 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11658 final Acceleration bz = new Acceleration(biasZ,
11659 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11660
11661 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11662 final double latitude = Math.toRadians(
11663 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11664 final double longitude = Math.toRadians(
11665 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11666 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11667 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11668 final NEDVelocity nedVelocity = new NEDVelocity();
11669 final ECEFPosition ecefPosition = new ECEFPosition();
11670 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11671 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11672 ecefPosition, ecefVelocity);
11673
11674 final KnownPositionAccelerometerCalibrator calibrator =
11675 new KnownPositionAccelerometerCalibrator(ecefPosition,
11676 true, bx, by, bz);
11677
11678
11679 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11680 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11681 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11682 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11683 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11684 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11685 final Acceleration bx2 = new Acceleration(0.0,
11686 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11687 calibrator.getInitialBiasXAsAcceleration(bx2);
11688 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11689 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11690 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11691 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11692 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11693 final Acceleration by2 = new Acceleration(0.0,
11694 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11695 calibrator.getInitialBiasYAsAcceleration(by2);
11696 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11697 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11698 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11699 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11700 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11701 final Acceleration bz2 = new Acceleration(0.0,
11702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11703 calibrator.getInitialBiasZAsAcceleration(bz2);
11704 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11705 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11706 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11707 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11708 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11709 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11710 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11711 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11712 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11713 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11714 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11715 final double[] bias1 = calibrator.getInitialBias();
11716 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11717 final double[] bias2 = new double[3];
11718 calibrator.getInitialBias(bias2);
11719 assertArrayEquals(bias1, bias2, 0.0);
11720 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11721 assertEquals(b1, ba);
11722 final Matrix b2 = new Matrix(3, 1);
11723 calibrator.getInitialBiasAsMatrix(b2);
11724 assertEquals(b1, b2);
11725 final Matrix ma1 = calibrator.getInitialMa();
11726 assertEquals(ma1, new Matrix(3, 3));
11727 final Matrix ma2 = new Matrix(3, 3);
11728 calibrator.getInitialMa(ma2);
11729 assertEquals(ma1, ma2);
11730 assertSame(calibrator.getEcefPosition(), ecefPosition);
11731 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11732 final NEDPosition nedPosition1 = new NEDPosition();
11733 assertTrue(calibrator.getNedPosition(nedPosition1));
11734 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11735 assertNull(calibrator.getMeasurements());
11736 assertTrue(calibrator.isCommonAxisUsed());
11737 assertNull(calibrator.getListener());
11738 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11739 assertFalse(calibrator.isReady());
11740 assertFalse(calibrator.isRunning());
11741 assertNull(calibrator.getEstimatedBiases());
11742 assertFalse(calibrator.getEstimatedBiases(null));
11743 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11744 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11745 assertNull(calibrator.getEstimatedBiasFx());
11746 assertNull(calibrator.getEstimatedBiasFy());
11747 assertNull(calibrator.getEstimatedBiasFz());
11748 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11749 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11750 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11751 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11752 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11753 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11754 assertNull(calibrator.getEstimatedMa());
11755 assertNull(calibrator.getEstimatedSx());
11756 assertNull(calibrator.getEstimatedSy());
11757 assertNull(calibrator.getEstimatedSz());
11758 assertNull(calibrator.getEstimatedMxy());
11759 assertNull(calibrator.getEstimatedMxz());
11760 assertNull(calibrator.getEstimatedMyx());
11761 assertNull(calibrator.getEstimatedMyz());
11762 assertNull(calibrator.getEstimatedMzx());
11763 assertNull(calibrator.getEstimatedMzy());
11764 assertNull(calibrator.getEstimatedCovariance());
11765 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11766 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11767 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11768 assertNotNull(calibrator.getGroundTruthGravityNorm());
11769 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11770 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11771 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11772 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11773 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11774 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11775 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11776 }
11777
11778 @Test
11779 public void testConstructor100() throws WrongSizeException {
11780 final Matrix ba = generateBa();
11781 final double biasX = ba.getElementAtIndex(0);
11782 final double biasY = ba.getElementAtIndex(1);
11783 final double biasZ = ba.getElementAtIndex(2);
11784
11785 final Acceleration bx = new Acceleration(biasX,
11786 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11787 final Acceleration by = new Acceleration(biasY,
11788 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11789 final Acceleration bz = new Acceleration(biasZ,
11790 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11791
11792 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11793 final double latitude = Math.toRadians(
11794 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11795 final double longitude = Math.toRadians(
11796 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11797 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11798 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11799 final NEDVelocity nedVelocity = new NEDVelocity();
11800 final ECEFPosition ecefPosition = new ECEFPosition();
11801 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11802 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11803 ecefPosition, ecefVelocity);
11804
11805 final KnownPositionAccelerometerCalibrator calibrator =
11806 new KnownPositionAccelerometerCalibrator(ecefPosition,
11807 true, bx, by, bz, this);
11808
11809
11810 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11811 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11812 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11813 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11814 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11815 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11816 final Acceleration bx2 = new Acceleration(0.0,
11817 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11818 calibrator.getInitialBiasXAsAcceleration(bx2);
11819 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11820 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11821 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11822 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11823 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11824 final Acceleration by2 = new Acceleration(0.0,
11825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11826 calibrator.getInitialBiasYAsAcceleration(by2);
11827 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11828 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11829 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11830 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11831 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11832 final Acceleration bz2 = new Acceleration(0.0,
11833 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11834 calibrator.getInitialBiasZAsAcceleration(bz2);
11835 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11836 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11837 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11838 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11839 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11840 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11841 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11842 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11843 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11844 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11845 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11846 final double[] bias1 = calibrator.getInitialBias();
11847 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11848 final double[] bias2 = new double[3];
11849 calibrator.getInitialBias(bias2);
11850 assertArrayEquals(bias1, bias2, 0.0);
11851 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11852 assertEquals(b1, ba);
11853 final Matrix b2 = new Matrix(3, 1);
11854 calibrator.getInitialBiasAsMatrix(b2);
11855 assertEquals(b1, b2);
11856 final Matrix ma1 = calibrator.getInitialMa();
11857 assertEquals(ma1, new Matrix(3, 3));
11858 final Matrix ma2 = new Matrix(3, 3);
11859 calibrator.getInitialMa(ma2);
11860 assertEquals(ma1, ma2);
11861 assertSame(calibrator.getEcefPosition(), ecefPosition);
11862 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11863 final NEDPosition nedPosition1 = new NEDPosition();
11864 assertTrue(calibrator.getNedPosition(nedPosition1));
11865 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11866 assertNull(calibrator.getMeasurements());
11867 assertTrue(calibrator.isCommonAxisUsed());
11868 assertSame(calibrator.getListener(), this);
11869 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11870 assertFalse(calibrator.isReady());
11871 assertFalse(calibrator.isRunning());
11872 assertNull(calibrator.getEstimatedBiases());
11873 assertFalse(calibrator.getEstimatedBiases(null));
11874 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11875 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11876 assertNull(calibrator.getEstimatedBiasFx());
11877 assertNull(calibrator.getEstimatedBiasFy());
11878 assertNull(calibrator.getEstimatedBiasFz());
11879 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11880 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11881 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11882 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11883 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11884 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11885 assertNull(calibrator.getEstimatedMa());
11886 assertNull(calibrator.getEstimatedSx());
11887 assertNull(calibrator.getEstimatedSy());
11888 assertNull(calibrator.getEstimatedSz());
11889 assertNull(calibrator.getEstimatedMxy());
11890 assertNull(calibrator.getEstimatedMxz());
11891 assertNull(calibrator.getEstimatedMyx());
11892 assertNull(calibrator.getEstimatedMyz());
11893 assertNull(calibrator.getEstimatedMzx());
11894 assertNull(calibrator.getEstimatedMzy());
11895 assertNull(calibrator.getEstimatedCovariance());
11896 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11897 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11898 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11899 assertNotNull(calibrator.getGroundTruthGravityNorm());
11900 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11901 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11902 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11903 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11904 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11905 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11906 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11907 }
11908
11909 @Test
11910 public void testConstructor101() throws WrongSizeException {
11911 final Collection<StandardDeviationBodyKinematics> measurements =
11912 Collections.emptyList();
11913
11914 final Matrix ba = generateBa();
11915 final double biasX = ba.getElementAtIndex(0);
11916 final double biasY = ba.getElementAtIndex(1);
11917 final double biasZ = ba.getElementAtIndex(2);
11918
11919 final Acceleration bx = new Acceleration(biasX,
11920 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11921 final Acceleration by = new Acceleration(biasY,
11922 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11923 final Acceleration bz = new Acceleration(biasZ,
11924 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11925
11926 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11927 final double latitude = Math.toRadians(
11928 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11929 final double longitude = Math.toRadians(
11930 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11931 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11932 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11933 final NEDVelocity nedVelocity = new NEDVelocity();
11934 final ECEFPosition ecefPosition = new ECEFPosition();
11935 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11936 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11937 ecefPosition, ecefVelocity);
11938
11939 final KnownPositionAccelerometerCalibrator calibrator =
11940 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11941 true, bx, by, bz);
11942
11943
11944 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11945 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11946 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11947 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11948 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11949 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11950 final Acceleration bx2 = new Acceleration(0.0,
11951 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11952 calibrator.getInitialBiasXAsAcceleration(bx2);
11953 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11954 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11955 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11956 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11957 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11958 final Acceleration by2 = new Acceleration(0.0,
11959 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11960 calibrator.getInitialBiasYAsAcceleration(by2);
11961 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11962 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11963 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11964 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11965 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11966 final Acceleration bz2 = new Acceleration(0.0,
11967 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11968 calibrator.getInitialBiasZAsAcceleration(bz2);
11969 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11970 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11971 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11972 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11973 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11974 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11975 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11976 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11977 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11978 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11979 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11980 final double[] bias1 = calibrator.getInitialBias();
11981 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11982 final double[] bias2 = new double[3];
11983 calibrator.getInitialBias(bias2);
11984 assertArrayEquals(bias1, bias2, 0.0);
11985 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11986 assertEquals(b1, ba);
11987 final Matrix b2 = new Matrix(3, 1);
11988 calibrator.getInitialBiasAsMatrix(b2);
11989 assertEquals(b1, b2);
11990 final Matrix ma1 = calibrator.getInitialMa();
11991 assertEquals(ma1, new Matrix(3, 3));
11992 final Matrix ma2 = new Matrix(3, 3);
11993 calibrator.getInitialMa(ma2);
11994 assertEquals(ma1, ma2);
11995 assertSame(calibrator.getEcefPosition(), ecefPosition);
11996 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11997 final NEDPosition nedPosition1 = new NEDPosition();
11998 assertTrue(calibrator.getNedPosition(nedPosition1));
11999 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12000 assertSame(calibrator.getMeasurements(), measurements);
12001 assertTrue(calibrator.isCommonAxisUsed());
12002 assertNull(calibrator.getListener());
12003 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12004 assertFalse(calibrator.isReady());
12005 assertFalse(calibrator.isRunning());
12006 assertNull(calibrator.getEstimatedBiases());
12007 assertFalse(calibrator.getEstimatedBiases(null));
12008 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12009 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12010 assertNull(calibrator.getEstimatedBiasFx());
12011 assertNull(calibrator.getEstimatedBiasFy());
12012 assertNull(calibrator.getEstimatedBiasFz());
12013 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12014 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12015 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12016 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12017 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12018 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12019 assertNull(calibrator.getEstimatedMa());
12020 assertNull(calibrator.getEstimatedSx());
12021 assertNull(calibrator.getEstimatedSy());
12022 assertNull(calibrator.getEstimatedSz());
12023 assertNull(calibrator.getEstimatedMxy());
12024 assertNull(calibrator.getEstimatedMxz());
12025 assertNull(calibrator.getEstimatedMyx());
12026 assertNull(calibrator.getEstimatedMyz());
12027 assertNull(calibrator.getEstimatedMzx());
12028 assertNull(calibrator.getEstimatedMzy());
12029 assertNull(calibrator.getEstimatedCovariance());
12030 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12031 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12032 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12033 assertNotNull(calibrator.getGroundTruthGravityNorm());
12034 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12035 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12036 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12037 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12038 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12039 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12040 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12041 }
12042
12043 @Test
12044 public void testConstructor102() throws WrongSizeException {
12045 final Collection<StandardDeviationBodyKinematics> measurements =
12046 Collections.emptyList();
12047
12048 final Matrix ba = generateBa();
12049 final double biasX = ba.getElementAtIndex(0);
12050 final double biasY = ba.getElementAtIndex(1);
12051 final double biasZ = ba.getElementAtIndex(2);
12052
12053 final Acceleration bx = new Acceleration(biasX,
12054 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12055 final Acceleration by = new Acceleration(biasY,
12056 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12057 final Acceleration bz = new Acceleration(biasZ,
12058 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12059
12060 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12061 final double latitude = Math.toRadians(
12062 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12063 final double longitude = Math.toRadians(
12064 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12065 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12066 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12067 final NEDVelocity nedVelocity = new NEDVelocity();
12068 final ECEFPosition ecefPosition = new ECEFPosition();
12069 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12070 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12071 ecefPosition, ecefVelocity);
12072
12073 final KnownPositionAccelerometerCalibrator calibrator =
12074 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12075 true, bx, by, bz, this);
12076
12077
12078 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12079 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12080 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12081 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12082 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12083 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12084 final Acceleration bx2 = new Acceleration(0.0,
12085 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12086 calibrator.getInitialBiasXAsAcceleration(bx2);
12087 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12088 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12089 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12090 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12091 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12092 final Acceleration by2 = new Acceleration(0.0,
12093 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12094 calibrator.getInitialBiasYAsAcceleration(by2);
12095 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12096 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12097 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12098 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12099 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12100 final Acceleration bz2 = new Acceleration(0.0,
12101 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12102 calibrator.getInitialBiasZAsAcceleration(bz2);
12103 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12104 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12105 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
12106 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
12107 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
12108 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12109 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12110 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12111 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12112 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12113 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12114 final double[] bias1 = calibrator.getInitialBias();
12115 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12116 final double[] bias2 = new double[3];
12117 calibrator.getInitialBias(bias2);
12118 assertArrayEquals(bias1, bias2, 0.0);
12119 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12120 assertEquals(b1, ba);
12121 final Matrix b2 = new Matrix(3, 1);
12122 calibrator.getInitialBiasAsMatrix(b2);
12123 assertEquals(b1, b2);
12124 final Matrix ma1 = calibrator.getInitialMa();
12125 assertEquals(ma1, new Matrix(3, 3));
12126 final Matrix ma2 = new Matrix(3, 3);
12127 calibrator.getInitialMa(ma2);
12128 assertEquals(ma1, ma2);
12129 assertSame(calibrator.getEcefPosition(), ecefPosition);
12130 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12131 final NEDPosition nedPosition1 = new NEDPosition();
12132 assertTrue(calibrator.getNedPosition(nedPosition1));
12133 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12134 assertSame(calibrator.getMeasurements(), measurements);
12135 assertTrue(calibrator.isCommonAxisUsed());
12136 assertSame(calibrator.getListener(), this);
12137 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12138 assertFalse(calibrator.isReady());
12139 assertFalse(calibrator.isRunning());
12140 assertNull(calibrator.getEstimatedBiases());
12141 assertFalse(calibrator.getEstimatedBiases(null));
12142 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12143 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12144 assertNull(calibrator.getEstimatedBiasFx());
12145 assertNull(calibrator.getEstimatedBiasFy());
12146 assertNull(calibrator.getEstimatedBiasFz());
12147 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12148 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12149 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12150 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12151 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12152 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12153 assertNull(calibrator.getEstimatedMa());
12154 assertNull(calibrator.getEstimatedSx());
12155 assertNull(calibrator.getEstimatedSy());
12156 assertNull(calibrator.getEstimatedSz());
12157 assertNull(calibrator.getEstimatedMxy());
12158 assertNull(calibrator.getEstimatedMxz());
12159 assertNull(calibrator.getEstimatedMyx());
12160 assertNull(calibrator.getEstimatedMyz());
12161 assertNull(calibrator.getEstimatedMzx());
12162 assertNull(calibrator.getEstimatedMzy());
12163 assertNull(calibrator.getEstimatedCovariance());
12164 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12165 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12166 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12167 assertNotNull(calibrator.getGroundTruthGravityNorm());
12168 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12169 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12170 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12171 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12172 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12173 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12174 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12175 }
12176
12177 @Test
12178 public void testConstructor103() throws WrongSizeException {
12179 final Matrix ba = generateBa();
12180 final double biasX = ba.getElementAtIndex(0);
12181 final double biasY = ba.getElementAtIndex(1);
12182 final double biasZ = ba.getElementAtIndex(2);
12183
12184 final Matrix ma = generateMaCommonAxis();
12185 final double sx = ma.getElementAt(0, 0);
12186 final double sy = ma.getElementAt(1, 1);
12187 final double sz = ma.getElementAt(2, 2);
12188
12189 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12190 final double latitude = Math.toRadians(
12191 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12192 final double longitude = Math.toRadians(
12193 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12194 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12195 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12196 final NEDVelocity nedVelocity = new NEDVelocity();
12197 final ECEFPosition ecefPosition = new ECEFPosition();
12198 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12199 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12200 ecefPosition, ecefVelocity);
12201
12202 final KnownPositionAccelerometerCalibrator calibrator =
12203 new KnownPositionAccelerometerCalibrator(ecefPosition,
12204 biasX, biasY, biasZ, sx, sy, sz);
12205
12206
12207 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12208 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12209 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12210 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12211 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12212 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12213 final Acceleration bx2 = new Acceleration(0.0,
12214 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12215 calibrator.getInitialBiasXAsAcceleration(bx2);
12216 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12217 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12218 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12219 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12220 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12221 final Acceleration by2 = new Acceleration(0.0,
12222 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12223 calibrator.getInitialBiasYAsAcceleration(by2);
12224 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12225 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12226 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12227 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12228 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12229 final Acceleration bz2 = new Acceleration(0.0,
12230 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12231 calibrator.getInitialBiasZAsAcceleration(bz2);
12232 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12233 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12234 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12235 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12236 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12237 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12238 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12239 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12240 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12241 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12242 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12243 final double[] bias1 = calibrator.getInitialBias();
12244 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12245 final double[] bias2 = new double[3];
12246 calibrator.getInitialBias(bias2);
12247 assertArrayEquals(bias1, bias2, 0.0);
12248 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12249 assertEquals(b1, ba);
12250 final Matrix b2 = new Matrix(3, 1);
12251 calibrator.getInitialBiasAsMatrix(b2);
12252 assertEquals(b1, b2);
12253 final Matrix ma1 = calibrator.getInitialMa();
12254 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12255 final Matrix ma2 = new Matrix(3, 3);
12256 calibrator.getInitialMa(ma2);
12257 assertEquals(ma1, ma2);
12258 assertSame(calibrator.getEcefPosition(), ecefPosition);
12259 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12260 final NEDPosition nedPosition1 = new NEDPosition();
12261 assertTrue(calibrator.getNedPosition(nedPosition1));
12262 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12263 assertNull(calibrator.getMeasurements());
12264 assertFalse(calibrator.isCommonAxisUsed());
12265 assertNull(calibrator.getListener());
12266 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12267 assertFalse(calibrator.isReady());
12268 assertFalse(calibrator.isRunning());
12269 assertNull(calibrator.getEstimatedBiases());
12270 assertFalse(calibrator.getEstimatedBiases(null));
12271 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12272 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12273 assertNull(calibrator.getEstimatedBiasFx());
12274 assertNull(calibrator.getEstimatedBiasFy());
12275 assertNull(calibrator.getEstimatedBiasFz());
12276 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12277 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12278 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12279 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12280 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12281 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12282 assertNull(calibrator.getEstimatedMa());
12283 assertNull(calibrator.getEstimatedSx());
12284 assertNull(calibrator.getEstimatedSy());
12285 assertNull(calibrator.getEstimatedSz());
12286 assertNull(calibrator.getEstimatedMxy());
12287 assertNull(calibrator.getEstimatedMxz());
12288 assertNull(calibrator.getEstimatedMyx());
12289 assertNull(calibrator.getEstimatedMyz());
12290 assertNull(calibrator.getEstimatedMzx());
12291 assertNull(calibrator.getEstimatedMzy());
12292 assertNull(calibrator.getEstimatedCovariance());
12293 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12294 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12295 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12296 assertNotNull(calibrator.getGroundTruthGravityNorm());
12297 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12298 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12299 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12300 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12301 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12302 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12303 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12304 }
12305
12306 @Test
12307 public void testConstructor104() throws WrongSizeException {
12308 final Collection<StandardDeviationBodyKinematics> measurements =
12309 Collections.emptyList();
12310
12311 final Matrix ba = generateBa();
12312 final double biasX = ba.getElementAtIndex(0);
12313 final double biasY = ba.getElementAtIndex(1);
12314 final double biasZ = ba.getElementAtIndex(2);
12315
12316 final Matrix ma = generateMaCommonAxis();
12317 final double sx = ma.getElementAt(0, 0);
12318 final double sy = ma.getElementAt(1, 1);
12319 final double sz = ma.getElementAt(2, 2);
12320
12321 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12322 final double latitude = Math.toRadians(
12323 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12324 final double longitude = Math.toRadians(
12325 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12326 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12327 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12328 final NEDVelocity nedVelocity = new NEDVelocity();
12329 final ECEFPosition ecefPosition = new ECEFPosition();
12330 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12331 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12332 ecefPosition, ecefVelocity);
12333
12334 final KnownPositionAccelerometerCalibrator calibrator =
12335 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12336 biasX, biasY, biasZ, sx, sy, sz);
12337
12338
12339 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12340 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12341 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12342 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12343 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12344 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12345 final Acceleration bx2 = new Acceleration(0.0,
12346 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12347 calibrator.getInitialBiasXAsAcceleration(bx2);
12348 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12349 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12350 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12351 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12352 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12353 final Acceleration by2 = new Acceleration(0.0,
12354 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12355 calibrator.getInitialBiasYAsAcceleration(by2);
12356 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12357 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12358 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12359 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12360 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12361 final Acceleration bz2 = new Acceleration(0.0,
12362 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12363 calibrator.getInitialBiasZAsAcceleration(bz2);
12364 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12365 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12366 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12367 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12368 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12369 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12370 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12371 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12372 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12373 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12374 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12375 final double[] bias1 = calibrator.getInitialBias();
12376 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12377 final double[] bias2 = new double[3];
12378 calibrator.getInitialBias(bias2);
12379 assertArrayEquals(bias1, bias2, 0.0);
12380 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12381 assertEquals(b1, ba);
12382 final Matrix b2 = new Matrix(3, 1);
12383 calibrator.getInitialBiasAsMatrix(b2);
12384 assertEquals(b1, b2);
12385 final Matrix ma1 = calibrator.getInitialMa();
12386 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12387 final Matrix ma2 = new Matrix(3, 3);
12388 calibrator.getInitialMa(ma2);
12389 assertEquals(ma1, ma2);
12390 assertSame(calibrator.getEcefPosition(), ecefPosition);
12391 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12392 final NEDPosition nedPosition1 = new NEDPosition();
12393 assertTrue(calibrator.getNedPosition(nedPosition1));
12394 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12395 assertSame(calibrator.getMeasurements(), measurements);
12396 assertFalse(calibrator.isCommonAxisUsed());
12397 assertNull(calibrator.getListener());
12398 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12399 assertFalse(calibrator.isReady());
12400 assertFalse(calibrator.isRunning());
12401 assertNull(calibrator.getEstimatedBiases());
12402 assertFalse(calibrator.getEstimatedBiases(null));
12403 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12404 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12405 assertNull(calibrator.getEstimatedBiasFx());
12406 assertNull(calibrator.getEstimatedBiasFy());
12407 assertNull(calibrator.getEstimatedBiasFz());
12408 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12409 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12410 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12411 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12412 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12413 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12414 assertNull(calibrator.getEstimatedMa());
12415 assertNull(calibrator.getEstimatedSx());
12416 assertNull(calibrator.getEstimatedSy());
12417 assertNull(calibrator.getEstimatedSz());
12418 assertNull(calibrator.getEstimatedMxy());
12419 assertNull(calibrator.getEstimatedMxz());
12420 assertNull(calibrator.getEstimatedMyx());
12421 assertNull(calibrator.getEstimatedMyz());
12422 assertNull(calibrator.getEstimatedMzx());
12423 assertNull(calibrator.getEstimatedMzy());
12424 assertNull(calibrator.getEstimatedCovariance());
12425 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12426 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12427 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12428 assertNotNull(calibrator.getGroundTruthGravityNorm());
12429 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12430 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12431 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12432 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12433 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12434 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12435 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12436 }
12437
12438 @Test
12439 public void testConstructor105() throws WrongSizeException {
12440 final Collection<StandardDeviationBodyKinematics> measurements =
12441 Collections.emptyList();
12442
12443 final Matrix ba = generateBa();
12444 final double biasX = ba.getElementAtIndex(0);
12445 final double biasY = ba.getElementAtIndex(1);
12446 final double biasZ = ba.getElementAtIndex(2);
12447
12448 final Matrix ma = generateMaCommonAxis();
12449 final double sx = ma.getElementAt(0, 0);
12450 final double sy = ma.getElementAt(1, 1);
12451 final double sz = ma.getElementAt(2, 2);
12452
12453 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12454 final double latitude = Math.toRadians(
12455 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12456 final double longitude = Math.toRadians(
12457 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12458 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12459 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12460 final NEDVelocity nedVelocity = new NEDVelocity();
12461 final ECEFPosition ecefPosition = new ECEFPosition();
12462 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12463 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12464 ecefPosition, ecefVelocity);
12465
12466 final KnownPositionAccelerometerCalibrator calibrator =
12467 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12468 biasX, biasY, biasZ, sx, sy, sz, this);
12469
12470
12471 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12472 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12473 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12474 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12475 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12476 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12477 final Acceleration bx2 = new Acceleration(0.0,
12478 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12479 calibrator.getInitialBiasXAsAcceleration(bx2);
12480 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12481 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12482 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12483 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12484 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12485 final Acceleration by2 = new Acceleration(0.0,
12486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12487 calibrator.getInitialBiasYAsAcceleration(by2);
12488 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12489 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12490 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12491 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12492 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12493 final Acceleration bz2 = new Acceleration(0.0,
12494 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12495 calibrator.getInitialBiasZAsAcceleration(bz2);
12496 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12497 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12498 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12499 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12500 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12501 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12502 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12503 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12504 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12505 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12506 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12507 final double[] bias1 = calibrator.getInitialBias();
12508 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12509 final double[] bias2 = new double[3];
12510 calibrator.getInitialBias(bias2);
12511 assertArrayEquals(bias1, bias2, 0.0);
12512 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12513 assertEquals(b1, ba);
12514 final Matrix b2 = new Matrix(3, 1);
12515 calibrator.getInitialBiasAsMatrix(b2);
12516 assertEquals(b1, b2);
12517 final Matrix ma1 = calibrator.getInitialMa();
12518 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12519 final Matrix ma2 = new Matrix(3, 3);
12520 calibrator.getInitialMa(ma2);
12521 assertEquals(ma1, ma2);
12522 assertSame(calibrator.getEcefPosition(), ecefPosition);
12523 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12524 final NEDPosition nedPosition1 = new NEDPosition();
12525 assertTrue(calibrator.getNedPosition(nedPosition1));
12526 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12527 assertSame(calibrator.getMeasurements(), measurements);
12528 assertFalse(calibrator.isCommonAxisUsed());
12529 assertSame(calibrator.getListener(), this);
12530 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12531 assertFalse(calibrator.isReady());
12532 assertFalse(calibrator.isRunning());
12533 assertNull(calibrator.getEstimatedBiases());
12534 assertFalse(calibrator.getEstimatedBiases(null));
12535 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12536 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12537 assertNull(calibrator.getEstimatedBiasFx());
12538 assertNull(calibrator.getEstimatedBiasFy());
12539 assertNull(calibrator.getEstimatedBiasFz());
12540 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12541 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12542 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12543 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12544 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12545 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12546 assertNull(calibrator.getEstimatedMa());
12547 assertNull(calibrator.getEstimatedSx());
12548 assertNull(calibrator.getEstimatedSy());
12549 assertNull(calibrator.getEstimatedSz());
12550 assertNull(calibrator.getEstimatedMxy());
12551 assertNull(calibrator.getEstimatedMxz());
12552 assertNull(calibrator.getEstimatedMyx());
12553 assertNull(calibrator.getEstimatedMyz());
12554 assertNull(calibrator.getEstimatedMzx());
12555 assertNull(calibrator.getEstimatedMzy());
12556 assertNull(calibrator.getEstimatedCovariance());
12557 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12558 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12559 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12560 assertNotNull(calibrator.getGroundTruthGravityNorm());
12561 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12562 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12563 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12564 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12565 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12566 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12567 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12568 }
12569
12570 @Test
12571 public void testConstructor106() throws WrongSizeException {
12572 final Matrix ba = generateBa();
12573 final double biasX = ba.getElementAtIndex(0);
12574 final double biasY = ba.getElementAtIndex(1);
12575 final double biasZ = ba.getElementAtIndex(2);
12576
12577 final Matrix ma = generateMaCommonAxis();
12578 final double sx = ma.getElementAt(0, 0);
12579 final double sy = ma.getElementAt(1, 1);
12580 final double sz = ma.getElementAt(2, 2);
12581
12582 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12583 final double latitude = Math.toRadians(
12584 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12585 final double longitude = Math.toRadians(
12586 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12587 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12588 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12589 final NEDVelocity nedVelocity = new NEDVelocity();
12590 final ECEFPosition ecefPosition = new ECEFPosition();
12591 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12592 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12593 ecefPosition, ecefVelocity);
12594
12595 final KnownPositionAccelerometerCalibrator calibrator =
12596 new KnownPositionAccelerometerCalibrator(ecefPosition,
12597 true, biasX, biasY, biasZ, sx, sy, sz);
12598
12599
12600 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12601 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12602 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12603 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12604 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12605 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12606 final Acceleration bx2 = new Acceleration(0.0,
12607 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12608 calibrator.getInitialBiasXAsAcceleration(bx2);
12609 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12610 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12611 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12612 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12613 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12614 final Acceleration by2 = new Acceleration(0.0,
12615 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12616 calibrator.getInitialBiasYAsAcceleration(by2);
12617 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12618 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12619 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12620 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12621 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12622 final Acceleration bz2 = new Acceleration(0.0,
12623 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12624 calibrator.getInitialBiasZAsAcceleration(bz2);
12625 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12626 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12627 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12628 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12629 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12630 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12631 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12632 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12633 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12634 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12635 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12636 final double[] bias1 = calibrator.getInitialBias();
12637 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12638 final double[] bias2 = new double[3];
12639 calibrator.getInitialBias(bias2);
12640 assertArrayEquals(bias1, bias2, 0.0);
12641 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12642 assertEquals(b1, ba);
12643 final Matrix b2 = new Matrix(3, 1);
12644 calibrator.getInitialBiasAsMatrix(b2);
12645 assertEquals(b1, b2);
12646 final Matrix ma1 = calibrator.getInitialMa();
12647 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12648 final Matrix ma2 = new Matrix(3, 3);
12649 calibrator.getInitialMa(ma2);
12650 assertEquals(ma1, ma2);
12651 assertSame(calibrator.getEcefPosition(), ecefPosition);
12652 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12653 final NEDPosition nedPosition1 = new NEDPosition();
12654 assertTrue(calibrator.getNedPosition(nedPosition1));
12655 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12656 assertNull(calibrator.getMeasurements());
12657 assertTrue(calibrator.isCommonAxisUsed());
12658 assertNull(calibrator.getListener());
12659 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12660 assertFalse(calibrator.isReady());
12661 assertFalse(calibrator.isRunning());
12662 assertNull(calibrator.getEstimatedBiases());
12663 assertFalse(calibrator.getEstimatedBiases(null));
12664 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12665 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12666 assertNull(calibrator.getEstimatedBiasFx());
12667 assertNull(calibrator.getEstimatedBiasFy());
12668 assertNull(calibrator.getEstimatedBiasFz());
12669 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12670 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12671 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12672 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12673 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12674 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12675 assertNull(calibrator.getEstimatedMa());
12676 assertNull(calibrator.getEstimatedSx());
12677 assertNull(calibrator.getEstimatedSy());
12678 assertNull(calibrator.getEstimatedSz());
12679 assertNull(calibrator.getEstimatedMxy());
12680 assertNull(calibrator.getEstimatedMxz());
12681 assertNull(calibrator.getEstimatedMyx());
12682 assertNull(calibrator.getEstimatedMyz());
12683 assertNull(calibrator.getEstimatedMzx());
12684 assertNull(calibrator.getEstimatedMzy());
12685 assertNull(calibrator.getEstimatedCovariance());
12686 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12687 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12688 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12689 assertNotNull(calibrator.getGroundTruthGravityNorm());
12690 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12691 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12692 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12693 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12694 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12695 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12696 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12697 }
12698
12699 @Test
12700 public void testConstructor107() throws WrongSizeException {
12701 final Matrix ba = generateBa();
12702 final double biasX = ba.getElementAtIndex(0);
12703 final double biasY = ba.getElementAtIndex(1);
12704 final double biasZ = ba.getElementAtIndex(2);
12705
12706 final Matrix ma = generateMaCommonAxis();
12707 final double sx = ma.getElementAt(0, 0);
12708 final double sy = ma.getElementAt(1, 1);
12709 final double sz = ma.getElementAt(2, 2);
12710
12711 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12712 final double latitude = Math.toRadians(
12713 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12714 final double longitude = Math.toRadians(
12715 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12716 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12717 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12718 final NEDVelocity nedVelocity = new NEDVelocity();
12719 final ECEFPosition ecefPosition = new ECEFPosition();
12720 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12721 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12722 ecefPosition, ecefVelocity);
12723
12724 final KnownPositionAccelerometerCalibrator calibrator =
12725 new KnownPositionAccelerometerCalibrator(ecefPosition,
12726 true, biasX, biasY, biasZ, sx, sy, sz,
12727 this);
12728
12729
12730 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12731 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12732 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12733 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12734 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12735 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12736 final Acceleration bx2 = new Acceleration(0.0,
12737 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12738 calibrator.getInitialBiasXAsAcceleration(bx2);
12739 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12740 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12741 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12742 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12743 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12744 final Acceleration by2 = new Acceleration(0.0,
12745 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12746 calibrator.getInitialBiasYAsAcceleration(by2);
12747 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12748 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12749 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12750 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12751 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12752 final Acceleration bz2 = new Acceleration(0.0,
12753 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12754 calibrator.getInitialBiasZAsAcceleration(bz2);
12755 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12756 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12757 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12758 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12759 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12760 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12761 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12762 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12763 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12764 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12765 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12766 final double[] bias1 = calibrator.getInitialBias();
12767 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12768 final double[] bias2 = new double[3];
12769 calibrator.getInitialBias(bias2);
12770 assertArrayEquals(bias1, bias2, 0.0);
12771 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12772 assertEquals(b1, ba);
12773 final Matrix b2 = new Matrix(3, 1);
12774 calibrator.getInitialBiasAsMatrix(b2);
12775 assertEquals(b1, b2);
12776 final Matrix ma1 = calibrator.getInitialMa();
12777 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12778 final Matrix ma2 = new Matrix(3, 3);
12779 calibrator.getInitialMa(ma2);
12780 assertEquals(ma1, ma2);
12781 assertSame(calibrator.getEcefPosition(), ecefPosition);
12782 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12783 final NEDPosition nedPosition1 = new NEDPosition();
12784 assertTrue(calibrator.getNedPosition(nedPosition1));
12785 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12786 assertNull(calibrator.getMeasurements());
12787 assertTrue(calibrator.isCommonAxisUsed());
12788 assertSame(calibrator.getListener(), this);
12789 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12790 assertFalse(calibrator.isReady());
12791 assertFalse(calibrator.isRunning());
12792 assertNull(calibrator.getEstimatedBiases());
12793 assertFalse(calibrator.getEstimatedBiases(null));
12794 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12795 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12796 assertNull(calibrator.getEstimatedBiasFx());
12797 assertNull(calibrator.getEstimatedBiasFy());
12798 assertNull(calibrator.getEstimatedBiasFz());
12799 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12800 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12801 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12802 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12803 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12804 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12805 assertNull(calibrator.getEstimatedMa());
12806 assertNull(calibrator.getEstimatedSx());
12807 assertNull(calibrator.getEstimatedSy());
12808 assertNull(calibrator.getEstimatedSz());
12809 assertNull(calibrator.getEstimatedMxy());
12810 assertNull(calibrator.getEstimatedMxz());
12811 assertNull(calibrator.getEstimatedMyx());
12812 assertNull(calibrator.getEstimatedMyz());
12813 assertNull(calibrator.getEstimatedMzx());
12814 assertNull(calibrator.getEstimatedMzy());
12815 assertNull(calibrator.getEstimatedCovariance());
12816 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12817 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12818 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12819 assertNotNull(calibrator.getGroundTruthGravityNorm());
12820 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12821 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12822 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12823 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12824 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12825 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12826 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12827 }
12828
12829 @Test
12830 public void testConstructor108() throws WrongSizeException {
12831 final Collection<StandardDeviationBodyKinematics> measurements =
12832 Collections.emptyList();
12833
12834 final Matrix ba = generateBa();
12835 final double biasX = ba.getElementAtIndex(0);
12836 final double biasY = ba.getElementAtIndex(1);
12837 final double biasZ = ba.getElementAtIndex(2);
12838
12839 final Matrix ma = generateMaCommonAxis();
12840 final double sx = ma.getElementAt(0, 0);
12841 final double sy = ma.getElementAt(1, 1);
12842 final double sz = ma.getElementAt(2, 2);
12843
12844 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12845 final double latitude = Math.toRadians(
12846 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12847 final double longitude = Math.toRadians(
12848 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12849 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12850 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12851 final NEDVelocity nedVelocity = new NEDVelocity();
12852 final ECEFPosition ecefPosition = new ECEFPosition();
12853 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12854 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12855 ecefPosition, ecefVelocity);
12856
12857 final KnownPositionAccelerometerCalibrator calibrator =
12858 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12859 true, biasX, biasY, biasZ, sx, sy, sz);
12860
12861
12862 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12863 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12864 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12865 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12866 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12867 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12868 final Acceleration bx2 = new Acceleration(0.0,
12869 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12870 calibrator.getInitialBiasXAsAcceleration(bx2);
12871 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12872 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12873 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12874 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12875 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12876 final Acceleration by2 = new Acceleration(0.0,
12877 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12878 calibrator.getInitialBiasYAsAcceleration(by2);
12879 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12880 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12881 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12882 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12883 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12884 final Acceleration bz2 = new Acceleration(0.0,
12885 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12886 calibrator.getInitialBiasZAsAcceleration(bz2);
12887 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12888 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12889 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12890 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12891 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12892 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12893 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12894 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12895 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12896 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12897 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12898 final double[] bias1 = calibrator.getInitialBias();
12899 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12900 final double[] bias2 = new double[3];
12901 calibrator.getInitialBias(bias2);
12902 assertArrayEquals(bias1, bias2, 0.0);
12903 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12904 assertEquals(b1, ba);
12905 final Matrix b2 = new Matrix(3, 1);
12906 calibrator.getInitialBiasAsMatrix(b2);
12907 assertEquals(b1, b2);
12908 final Matrix ma1 = calibrator.getInitialMa();
12909 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12910 final Matrix ma2 = new Matrix(3, 3);
12911 calibrator.getInitialMa(ma2);
12912 assertEquals(ma1, ma2);
12913 assertSame(calibrator.getEcefPosition(), ecefPosition);
12914 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12915 final NEDPosition nedPosition1 = new NEDPosition();
12916 assertTrue(calibrator.getNedPosition(nedPosition1));
12917 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12918 assertSame(calibrator.getMeasurements(), measurements);
12919 assertTrue(calibrator.isCommonAxisUsed());
12920 assertNull(calibrator.getListener());
12921 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12922 assertFalse(calibrator.isReady());
12923 assertFalse(calibrator.isRunning());
12924 assertNull(calibrator.getEstimatedBiases());
12925 assertFalse(calibrator.getEstimatedBiases(null));
12926 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12927 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12928 assertNull(calibrator.getEstimatedBiasFx());
12929 assertNull(calibrator.getEstimatedBiasFy());
12930 assertNull(calibrator.getEstimatedBiasFz());
12931 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12932 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12933 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12934 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12935 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12936 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12937 assertNull(calibrator.getEstimatedMa());
12938 assertNull(calibrator.getEstimatedSx());
12939 assertNull(calibrator.getEstimatedSy());
12940 assertNull(calibrator.getEstimatedSz());
12941 assertNull(calibrator.getEstimatedMxy());
12942 assertNull(calibrator.getEstimatedMxz());
12943 assertNull(calibrator.getEstimatedMyx());
12944 assertNull(calibrator.getEstimatedMyz());
12945 assertNull(calibrator.getEstimatedMzx());
12946 assertNull(calibrator.getEstimatedMzy());
12947 assertNull(calibrator.getEstimatedCovariance());
12948 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12949 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12950 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12951 assertNotNull(calibrator.getGroundTruthGravityNorm());
12952 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12953 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12954 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12955 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12956 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12957 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12958 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12959 }
12960
12961 @Test
12962 public void testConstructor109() throws WrongSizeException {
12963 final Collection<StandardDeviationBodyKinematics> measurements =
12964 Collections.emptyList();
12965
12966 final Matrix ba = generateBa();
12967 final double biasX = ba.getElementAtIndex(0);
12968 final double biasY = ba.getElementAtIndex(1);
12969 final double biasZ = ba.getElementAtIndex(2);
12970
12971 final Matrix ma = generateMaCommonAxis();
12972 final double sx = ma.getElementAt(0, 0);
12973 final double sy = ma.getElementAt(1, 1);
12974 final double sz = ma.getElementAt(2, 2);
12975
12976 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12977 final double latitude = Math.toRadians(
12978 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12979 final double longitude = Math.toRadians(
12980 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12981 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12982 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12983 final NEDVelocity nedVelocity = new NEDVelocity();
12984 final ECEFPosition ecefPosition = new ECEFPosition();
12985 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12986 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12987 ecefPosition, ecefVelocity);
12988
12989 final KnownPositionAccelerometerCalibrator calibrator =
12990 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12991 true, biasX, biasY, biasZ, sx, sy, sz,
12992 this);
12993
12994
12995 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12996 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12997 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12998 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12999 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13000 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13001 final Acceleration bx2 = new Acceleration(0.0,
13002 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13003 calibrator.getInitialBiasXAsAcceleration(bx2);
13004 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13005 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13006 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13007 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13008 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13009 final Acceleration by2 = new Acceleration(0.0,
13010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13011 calibrator.getInitialBiasYAsAcceleration(by2);
13012 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13013 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13014 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13015 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13016 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13017 final Acceleration bz2 = new Acceleration(0.0,
13018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13019 calibrator.getInitialBiasZAsAcceleration(bz2);
13020 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13021 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13022 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13023 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13024 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13025 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13026 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13027 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13028 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13029 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13030 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13031 final double[] bias1 = calibrator.getInitialBias();
13032 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13033 final double[] bias2 = new double[3];
13034 calibrator.getInitialBias(bias2);
13035 assertArrayEquals(bias1, bias2, 0.0);
13036 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13037 assertEquals(b1, ba);
13038 final Matrix b2 = new Matrix(3, 1);
13039 calibrator.getInitialBiasAsMatrix(b2);
13040 assertEquals(b1, b2);
13041 final Matrix ma1 = calibrator.getInitialMa();
13042 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13043 final Matrix ma2 = new Matrix(3, 3);
13044 calibrator.getInitialMa(ma2);
13045 assertEquals(ma1, ma2);
13046 assertSame(calibrator.getEcefPosition(), ecefPosition);
13047 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13048 final NEDPosition nedPosition1 = new NEDPosition();
13049 assertTrue(calibrator.getNedPosition(nedPosition1));
13050 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13051 assertSame(calibrator.getMeasurements(), measurements);
13052 assertTrue(calibrator.isCommonAxisUsed());
13053 assertSame(calibrator.getListener(), this);
13054 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13055 assertFalse(calibrator.isReady());
13056 assertFalse(calibrator.isRunning());
13057 assertNull(calibrator.getEstimatedBiases());
13058 assertFalse(calibrator.getEstimatedBiases(null));
13059 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13060 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13061 assertNull(calibrator.getEstimatedBiasFx());
13062 assertNull(calibrator.getEstimatedBiasFy());
13063 assertNull(calibrator.getEstimatedBiasFz());
13064 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13065 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13066 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13067 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13068 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13069 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13070 assertNull(calibrator.getEstimatedMa());
13071 assertNull(calibrator.getEstimatedSx());
13072 assertNull(calibrator.getEstimatedSy());
13073 assertNull(calibrator.getEstimatedSz());
13074 assertNull(calibrator.getEstimatedMxy());
13075 assertNull(calibrator.getEstimatedMxz());
13076 assertNull(calibrator.getEstimatedMyx());
13077 assertNull(calibrator.getEstimatedMyz());
13078 assertNull(calibrator.getEstimatedMzx());
13079 assertNull(calibrator.getEstimatedMzy());
13080 assertNull(calibrator.getEstimatedCovariance());
13081 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13082 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13083 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13084 assertNotNull(calibrator.getGroundTruthGravityNorm());
13085 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13086 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13087 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13088 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13089 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13090 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13091 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13092 }
13093
13094 @Test
13095 public void testConstructor110() throws WrongSizeException {
13096 final Matrix ba = generateBa();
13097 final double biasX = ba.getElementAtIndex(0);
13098 final double biasY = ba.getElementAtIndex(1);
13099 final double biasZ = ba.getElementAtIndex(2);
13100
13101 final Acceleration bx = new Acceleration(biasX,
13102 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13103 final Acceleration by = new Acceleration(biasY,
13104 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13105 final Acceleration bz = new Acceleration(biasZ,
13106 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13107
13108 final Matrix ma = generateMaCommonAxis();
13109 final double sx = ma.getElementAt(0, 0);
13110 final double sy = ma.getElementAt(1, 1);
13111 final double sz = ma.getElementAt(2, 2);
13112
13113 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13114 final double latitude = Math.toRadians(
13115 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13116 final double longitude = Math.toRadians(
13117 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13118 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13119 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13120 final NEDVelocity nedVelocity = new NEDVelocity();
13121 final ECEFPosition ecefPosition = new ECEFPosition();
13122 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13123 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13124 ecefPosition, ecefVelocity);
13125
13126 final KnownPositionAccelerometerCalibrator calibrator =
13127 new KnownPositionAccelerometerCalibrator(ecefPosition,
13128 bx, by, bz, sx, sy, sz);
13129
13130
13131 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13132 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13133 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13134 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13135 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13136 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13137 final Acceleration bx2 = new Acceleration(0.0,
13138 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13139 calibrator.getInitialBiasXAsAcceleration(bx2);
13140 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13141 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13142 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13143 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13144 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13145 final Acceleration by2 = new Acceleration(0.0,
13146 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13147 calibrator.getInitialBiasYAsAcceleration(by2);
13148 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13149 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13150 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13151 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13152 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13153 final Acceleration bz2 = new Acceleration(0.0,
13154 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13155 calibrator.getInitialBiasZAsAcceleration(bz2);
13156 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13157 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13158 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13159 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13160 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13161 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13162 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13163 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13164 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13165 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13166 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13167 final double[] bias1 = calibrator.getInitialBias();
13168 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13169 final double[] bias2 = new double[3];
13170 calibrator.getInitialBias(bias2);
13171 assertArrayEquals(bias1, bias2, 0.0);
13172 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13173 assertEquals(b1, ba);
13174 final Matrix b2 = new Matrix(3, 1);
13175 calibrator.getInitialBiasAsMatrix(b2);
13176 assertEquals(b1, b2);
13177 final Matrix ma1 = calibrator.getInitialMa();
13178 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13179 final Matrix ma2 = new Matrix(3, 3);
13180 calibrator.getInitialMa(ma2);
13181 assertEquals(ma1, ma2);
13182 assertSame(calibrator.getEcefPosition(), ecefPosition);
13183 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13184 final NEDPosition nedPosition1 = new NEDPosition();
13185 assertTrue(calibrator.getNedPosition(nedPosition1));
13186 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13187 assertNull(calibrator.getMeasurements());
13188 assertFalse(calibrator.isCommonAxisUsed());
13189 assertNull(calibrator.getListener());
13190 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13191 assertFalse(calibrator.isReady());
13192 assertFalse(calibrator.isRunning());
13193 assertNull(calibrator.getEstimatedBiases());
13194 assertFalse(calibrator.getEstimatedBiases(null));
13195 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13196 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13197 assertNull(calibrator.getEstimatedBiasFx());
13198 assertNull(calibrator.getEstimatedBiasFy());
13199 assertNull(calibrator.getEstimatedBiasFz());
13200 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13201 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13202 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13203 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13204 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13205 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13206 assertNull(calibrator.getEstimatedMa());
13207 assertNull(calibrator.getEstimatedSx());
13208 assertNull(calibrator.getEstimatedSy());
13209 assertNull(calibrator.getEstimatedSz());
13210 assertNull(calibrator.getEstimatedMxy());
13211 assertNull(calibrator.getEstimatedMxz());
13212 assertNull(calibrator.getEstimatedMyx());
13213 assertNull(calibrator.getEstimatedMyz());
13214 assertNull(calibrator.getEstimatedMzx());
13215 assertNull(calibrator.getEstimatedMzy());
13216 assertNull(calibrator.getEstimatedCovariance());
13217 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13218 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13219 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13220 assertNotNull(calibrator.getGroundTruthGravityNorm());
13221 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13222 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13223 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13224 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13225 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13226 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13227 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13228 }
13229
13230 @Test
13231 public void testConstructor111() throws WrongSizeException {
13232 final Matrix ba = generateBa();
13233 final double biasX = ba.getElementAtIndex(0);
13234 final double biasY = ba.getElementAtIndex(1);
13235 final double biasZ = ba.getElementAtIndex(2);
13236
13237 final Acceleration bx = new Acceleration(biasX,
13238 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13239 final Acceleration by = new Acceleration(biasY,
13240 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13241 final Acceleration bz = new Acceleration(biasZ,
13242 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13243
13244 final Matrix ma = generateMaCommonAxis();
13245 final double sx = ma.getElementAt(0, 0);
13246 final double sy = ma.getElementAt(1, 1);
13247 final double sz = ma.getElementAt(2, 2);
13248
13249 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13250 final double latitude = Math.toRadians(
13251 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13252 final double longitude = Math.toRadians(
13253 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13254 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13255 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13256 final NEDVelocity nedVelocity = new NEDVelocity();
13257 final ECEFPosition ecefPosition = new ECEFPosition();
13258 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13259 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13260 ecefPosition, ecefVelocity);
13261
13262 final KnownPositionAccelerometerCalibrator calibrator =
13263 new KnownPositionAccelerometerCalibrator(ecefPosition,
13264 bx, by, bz, sx, sy, sz, this);
13265
13266
13267 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13268 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13269 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13270 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13271 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13272 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13273 final Acceleration bx2 = new Acceleration(0.0,
13274 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13275 calibrator.getInitialBiasXAsAcceleration(bx2);
13276 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13277 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13278 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13279 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13280 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13281 final Acceleration by2 = new Acceleration(0.0,
13282 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13283 calibrator.getInitialBiasYAsAcceleration(by2);
13284 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13285 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13286 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13287 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13288 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13289 final Acceleration bz2 = new Acceleration(0.0,
13290 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13291 calibrator.getInitialBiasZAsAcceleration(bz2);
13292 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13293 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13294 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13295 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13296 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13297 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13298 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13299 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13300 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13301 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13302 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13303 final double[] bias1 = calibrator.getInitialBias();
13304 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13305 final double[] bias2 = new double[3];
13306 calibrator.getInitialBias(bias2);
13307 assertArrayEquals(bias1, bias2, 0.0);
13308 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13309 assertEquals(b1, ba);
13310 final Matrix b2 = new Matrix(3, 1);
13311 calibrator.getInitialBiasAsMatrix(b2);
13312 assertEquals(b1, b2);
13313 final Matrix ma1 = calibrator.getInitialMa();
13314 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13315 final Matrix ma2 = new Matrix(3, 3);
13316 calibrator.getInitialMa(ma2);
13317 assertEquals(ma1, ma2);
13318 assertSame(calibrator.getEcefPosition(), ecefPosition);
13319 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13320 final NEDPosition nedPosition1 = new NEDPosition();
13321 assertTrue(calibrator.getNedPosition(nedPosition1));
13322 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13323 assertNull(calibrator.getMeasurements());
13324 assertFalse(calibrator.isCommonAxisUsed());
13325 assertSame(calibrator.getListener(), this);
13326 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13327 assertFalse(calibrator.isReady());
13328 assertFalse(calibrator.isRunning());
13329 assertNull(calibrator.getEstimatedBiases());
13330 assertFalse(calibrator.getEstimatedBiases(null));
13331 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13332 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13333 assertNull(calibrator.getEstimatedBiasFx());
13334 assertNull(calibrator.getEstimatedBiasFy());
13335 assertNull(calibrator.getEstimatedBiasFz());
13336 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13337 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13338 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13339 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13340 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13341 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13342 assertNull(calibrator.getEstimatedMa());
13343 assertNull(calibrator.getEstimatedSx());
13344 assertNull(calibrator.getEstimatedSy());
13345 assertNull(calibrator.getEstimatedSz());
13346 assertNull(calibrator.getEstimatedMxy());
13347 assertNull(calibrator.getEstimatedMxz());
13348 assertNull(calibrator.getEstimatedMyx());
13349 assertNull(calibrator.getEstimatedMyz());
13350 assertNull(calibrator.getEstimatedMzx());
13351 assertNull(calibrator.getEstimatedMzy());
13352 assertNull(calibrator.getEstimatedCovariance());
13353 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13354 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13355 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13356 assertNotNull(calibrator.getGroundTruthGravityNorm());
13357 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13358 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13359 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13360 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13361 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13362 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13363 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13364 }
13365
13366 @Test
13367 public void testConstructor112() throws WrongSizeException {
13368 final Collection<StandardDeviationBodyKinematics> measurements =
13369 Collections.emptyList();
13370
13371 final Matrix ba = generateBa();
13372 final double biasX = ba.getElementAtIndex(0);
13373 final double biasY = ba.getElementAtIndex(1);
13374 final double biasZ = ba.getElementAtIndex(2);
13375
13376 final Acceleration bx = new Acceleration(biasX,
13377 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13378 final Acceleration by = new Acceleration(biasY,
13379 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13380 final Acceleration bz = new Acceleration(biasZ,
13381 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13382
13383 final Matrix ma = generateMaCommonAxis();
13384 final double sx = ma.getElementAt(0, 0);
13385 final double sy = ma.getElementAt(1, 1);
13386 final double sz = ma.getElementAt(2, 2);
13387
13388 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13389 final double latitude = Math.toRadians(
13390 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13391 final double longitude = Math.toRadians(
13392 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13393 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13394 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13395 final NEDVelocity nedVelocity = new NEDVelocity();
13396 final ECEFPosition ecefPosition = new ECEFPosition();
13397 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13398 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13399 ecefPosition, ecefVelocity);
13400
13401 final KnownPositionAccelerometerCalibrator calibrator =
13402 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13403 bx, by, bz, sx, sy, sz);
13404
13405
13406 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13407 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13408 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13409 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13410 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13411 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13412 final Acceleration bx2 = new Acceleration(0.0,
13413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13414 calibrator.getInitialBiasXAsAcceleration(bx2);
13415 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13416 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13417 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13418 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13419 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13420 final Acceleration by2 = new Acceleration(0.0,
13421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13422 calibrator.getInitialBiasYAsAcceleration(by2);
13423 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13424 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13425 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13426 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13427 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13428 final Acceleration bz2 = new Acceleration(0.0,
13429 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13430 calibrator.getInitialBiasZAsAcceleration(bz2);
13431 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13432 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13433 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13434 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13435 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13436 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13437 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13438 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13439 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13440 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13441 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13442 final double[] bias1 = calibrator.getInitialBias();
13443 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13444 final double[] bias2 = new double[3];
13445 calibrator.getInitialBias(bias2);
13446 assertArrayEquals(bias1, bias2, 0.0);
13447 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13448 assertEquals(b1, ba);
13449 final Matrix b2 = new Matrix(3, 1);
13450 calibrator.getInitialBiasAsMatrix(b2);
13451 assertEquals(b1, b2);
13452 final Matrix ma1 = calibrator.getInitialMa();
13453 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13454 final Matrix ma2 = new Matrix(3, 3);
13455 calibrator.getInitialMa(ma2);
13456 assertEquals(ma1, ma2);
13457 assertSame(calibrator.getEcefPosition(), ecefPosition);
13458 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13459 final NEDPosition nedPosition1 = new NEDPosition();
13460 assertTrue(calibrator.getNedPosition(nedPosition1));
13461 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13462 assertSame(calibrator.getMeasurements(), measurements);
13463 assertFalse(calibrator.isCommonAxisUsed());
13464 assertNull(calibrator.getListener());
13465 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13466 assertFalse(calibrator.isReady());
13467 assertFalse(calibrator.isRunning());
13468 assertNull(calibrator.getEstimatedBiases());
13469 assertFalse(calibrator.getEstimatedBiases(null));
13470 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13471 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13472 assertNull(calibrator.getEstimatedBiasFx());
13473 assertNull(calibrator.getEstimatedBiasFy());
13474 assertNull(calibrator.getEstimatedBiasFz());
13475 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13476 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13477 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13478 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13479 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13480 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13481 assertNull(calibrator.getEstimatedMa());
13482 assertNull(calibrator.getEstimatedSx());
13483 assertNull(calibrator.getEstimatedSy());
13484 assertNull(calibrator.getEstimatedSz());
13485 assertNull(calibrator.getEstimatedMxy());
13486 assertNull(calibrator.getEstimatedMxz());
13487 assertNull(calibrator.getEstimatedMyx());
13488 assertNull(calibrator.getEstimatedMyz());
13489 assertNull(calibrator.getEstimatedMzx());
13490 assertNull(calibrator.getEstimatedMzy());
13491 assertNull(calibrator.getEstimatedCovariance());
13492 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13493 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13494 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13495 assertNotNull(calibrator.getGroundTruthGravityNorm());
13496 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13497 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13498 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13499 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13500 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13501 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13502 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13503 }
13504
13505 @Test
13506 public void testConstructor113() throws WrongSizeException {
13507 final Collection<StandardDeviationBodyKinematics> measurements =
13508 Collections.emptyList();
13509
13510 final Matrix ba = generateBa();
13511 final double biasX = ba.getElementAtIndex(0);
13512 final double biasY = ba.getElementAtIndex(1);
13513 final double biasZ = ba.getElementAtIndex(2);
13514
13515 final Acceleration bx = new Acceleration(biasX,
13516 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13517 final Acceleration by = new Acceleration(biasY,
13518 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13519 final Acceleration bz = new Acceleration(biasZ,
13520 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13521
13522 final Matrix ma = generateMaCommonAxis();
13523 final double sx = ma.getElementAt(0, 0);
13524 final double sy = ma.getElementAt(1, 1);
13525 final double sz = ma.getElementAt(2, 2);
13526
13527 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13528 final double latitude = Math.toRadians(
13529 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13530 final double longitude = Math.toRadians(
13531 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13532 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13533 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13534 final NEDVelocity nedVelocity = new NEDVelocity();
13535 final ECEFPosition ecefPosition = new ECEFPosition();
13536 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13537 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13538 ecefPosition, ecefVelocity);
13539
13540 final KnownPositionAccelerometerCalibrator calibrator =
13541 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13542 bx, by, bz, sx, sy, sz, this);
13543
13544
13545 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13546 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13547 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13548 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13549 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13550 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13551 final Acceleration bx2 = new Acceleration(0.0,
13552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13553 calibrator.getInitialBiasXAsAcceleration(bx2);
13554 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13555 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13556 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13557 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13558 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13559 final Acceleration by2 = new Acceleration(0.0,
13560 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13561 calibrator.getInitialBiasYAsAcceleration(by2);
13562 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13563 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13564 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13565 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13566 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13567 final Acceleration bz2 = new Acceleration(0.0,
13568 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13569 calibrator.getInitialBiasZAsAcceleration(bz2);
13570 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13571 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13572 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13573 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13574 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13575 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13576 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13577 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13578 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13579 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13580 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13581 final double[] bias1 = calibrator.getInitialBias();
13582 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13583 final double[] bias2 = new double[3];
13584 calibrator.getInitialBias(bias2);
13585 assertArrayEquals(bias1, bias2, 0.0);
13586 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13587 assertEquals(b1, ba);
13588 final Matrix b2 = new Matrix(3, 1);
13589 calibrator.getInitialBiasAsMatrix(b2);
13590 assertEquals(b1, b2);
13591 final Matrix ma1 = calibrator.getInitialMa();
13592 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13593 final Matrix ma2 = new Matrix(3, 3);
13594 calibrator.getInitialMa(ma2);
13595 assertEquals(ma1, ma2);
13596 assertSame(calibrator.getEcefPosition(), ecefPosition);
13597 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13598 final NEDPosition nedPosition1 = new NEDPosition();
13599 assertTrue(calibrator.getNedPosition(nedPosition1));
13600 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13601 assertSame(calibrator.getMeasurements(), measurements);
13602 assertFalse(calibrator.isCommonAxisUsed());
13603 assertSame(calibrator.getListener(), this);
13604 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13605 assertFalse(calibrator.isReady());
13606 assertFalse(calibrator.isRunning());
13607 assertNull(calibrator.getEstimatedBiases());
13608 assertFalse(calibrator.getEstimatedBiases(null));
13609 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13610 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13611 assertNull(calibrator.getEstimatedBiasFx());
13612 assertNull(calibrator.getEstimatedBiasFy());
13613 assertNull(calibrator.getEstimatedBiasFz());
13614 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13615 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13616 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13617 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13618 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13619 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13620 assertNull(calibrator.getEstimatedMa());
13621 assertNull(calibrator.getEstimatedSx());
13622 assertNull(calibrator.getEstimatedSy());
13623 assertNull(calibrator.getEstimatedSz());
13624 assertNull(calibrator.getEstimatedMxy());
13625 assertNull(calibrator.getEstimatedMxz());
13626 assertNull(calibrator.getEstimatedMyx());
13627 assertNull(calibrator.getEstimatedMyz());
13628 assertNull(calibrator.getEstimatedMzx());
13629 assertNull(calibrator.getEstimatedMzy());
13630 assertNull(calibrator.getEstimatedCovariance());
13631 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13632 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13633 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13634 assertNotNull(calibrator.getGroundTruthGravityNorm());
13635 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13636 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13637 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13638 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13639 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13640 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13641 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13642 }
13643
13644 @Test
13645 public void testConstructor114() throws WrongSizeException {
13646 final Matrix ba = generateBa();
13647 final double biasX = ba.getElementAtIndex(0);
13648 final double biasY = ba.getElementAtIndex(1);
13649 final double biasZ = ba.getElementAtIndex(2);
13650
13651 final Acceleration bx = new Acceleration(biasX,
13652 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13653 final Acceleration by = new Acceleration(biasY,
13654 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13655 final Acceleration bz = new Acceleration(biasZ,
13656 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13657
13658 final Matrix ma = generateMaCommonAxis();
13659 final double sx = ma.getElementAt(0, 0);
13660 final double sy = ma.getElementAt(1, 1);
13661 final double sz = ma.getElementAt(2, 2);
13662
13663 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13664 final double latitude = Math.toRadians(
13665 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13666 final double longitude = Math.toRadians(
13667 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13668 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13669 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13670 final NEDVelocity nedVelocity = new NEDVelocity();
13671 final ECEFPosition ecefPosition = new ECEFPosition();
13672 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13673 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13674 ecefPosition, ecefVelocity);
13675
13676 final KnownPositionAccelerometerCalibrator calibrator =
13677 new KnownPositionAccelerometerCalibrator(ecefPosition,
13678 true, bx, by, bz, sx, sy, sz);
13679
13680
13681 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13682 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13683 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13684 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13685 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13686 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13687 final Acceleration bx2 = new Acceleration(0.0,
13688 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13689 calibrator.getInitialBiasXAsAcceleration(bx2);
13690 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13691 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13692 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13693 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13694 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13695 final Acceleration by2 = new Acceleration(0.0,
13696 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13697 calibrator.getInitialBiasYAsAcceleration(by2);
13698 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13699 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13700 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13701 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13702 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13703 final Acceleration bz2 = new Acceleration(0.0,
13704 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13705 calibrator.getInitialBiasZAsAcceleration(bz2);
13706 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13707 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13708 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13709 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13710 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13711 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13712 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13713 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13714 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13715 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13716 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13717 final double[] bias1 = calibrator.getInitialBias();
13718 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13719 final double[] bias2 = new double[3];
13720 calibrator.getInitialBias(bias2);
13721 assertArrayEquals(bias1, bias2, 0.0);
13722 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13723 assertEquals(b1, ba);
13724 final Matrix b2 = new Matrix(3, 1);
13725 calibrator.getInitialBiasAsMatrix(b2);
13726 assertEquals(b1, b2);
13727 final Matrix ma1 = calibrator.getInitialMa();
13728 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13729 final Matrix ma2 = new Matrix(3, 3);
13730 calibrator.getInitialMa(ma2);
13731 assertEquals(ma1, ma2);
13732 assertSame(calibrator.getEcefPosition(), ecefPosition);
13733 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13734 final NEDPosition nedPosition1 = new NEDPosition();
13735 assertTrue(calibrator.getNedPosition(nedPosition1));
13736 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13737 assertNull(calibrator.getMeasurements());
13738 assertTrue(calibrator.isCommonAxisUsed());
13739 assertNull(calibrator.getListener());
13740 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13741 assertFalse(calibrator.isReady());
13742 assertFalse(calibrator.isRunning());
13743 assertNull(calibrator.getEstimatedBiases());
13744 assertFalse(calibrator.getEstimatedBiases(null));
13745 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13746 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13747 assertNull(calibrator.getEstimatedBiasFx());
13748 assertNull(calibrator.getEstimatedBiasFy());
13749 assertNull(calibrator.getEstimatedBiasFz());
13750 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13751 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13752 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13753 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13754 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13755 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13756 assertNull(calibrator.getEstimatedMa());
13757 assertNull(calibrator.getEstimatedSx());
13758 assertNull(calibrator.getEstimatedSy());
13759 assertNull(calibrator.getEstimatedSz());
13760 assertNull(calibrator.getEstimatedMxy());
13761 assertNull(calibrator.getEstimatedMxz());
13762 assertNull(calibrator.getEstimatedMyx());
13763 assertNull(calibrator.getEstimatedMyz());
13764 assertNull(calibrator.getEstimatedMzx());
13765 assertNull(calibrator.getEstimatedMzy());
13766 assertNull(calibrator.getEstimatedCovariance());
13767 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13768 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13769 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13770 assertNotNull(calibrator.getGroundTruthGravityNorm());
13771 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13772 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13773 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13774 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13775 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13776 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13777 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13778 }
13779
13780 @Test
13781 public void testConstructor115() throws WrongSizeException {
13782 final Matrix ba = generateBa();
13783 final double biasX = ba.getElementAtIndex(0);
13784 final double biasY = ba.getElementAtIndex(1);
13785 final double biasZ = ba.getElementAtIndex(2);
13786
13787 final Acceleration bx = new Acceleration(biasX,
13788 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13789 final Acceleration by = new Acceleration(biasY,
13790 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13791 final Acceleration bz = new Acceleration(biasZ,
13792 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13793
13794 final Matrix ma = generateMaCommonAxis();
13795 final double sx = ma.getElementAt(0, 0);
13796 final double sy = ma.getElementAt(1, 1);
13797 final double sz = ma.getElementAt(2, 2);
13798
13799 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13800 final double latitude = Math.toRadians(
13801 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13802 final double longitude = Math.toRadians(
13803 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13804 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13805 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13806 final NEDVelocity nedVelocity = new NEDVelocity();
13807 final ECEFPosition ecefPosition = new ECEFPosition();
13808 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13809 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13810 ecefPosition, ecefVelocity);
13811
13812 final KnownPositionAccelerometerCalibrator calibrator =
13813 new KnownPositionAccelerometerCalibrator(ecefPosition,
13814 true, bx, by, bz, sx, sy, sz, this);
13815
13816
13817 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13818 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13819 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13820 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13821 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13822 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13823 final Acceleration bx2 = new Acceleration(0.0,
13824 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13825 calibrator.getInitialBiasXAsAcceleration(bx2);
13826 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13827 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13828 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13829 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13830 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13831 final Acceleration by2 = new Acceleration(0.0,
13832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13833 calibrator.getInitialBiasYAsAcceleration(by2);
13834 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13835 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13836 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13837 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13838 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13839 final Acceleration bz2 = new Acceleration(0.0,
13840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13841 calibrator.getInitialBiasZAsAcceleration(bz2);
13842 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13843 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13844 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13845 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13846 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13847 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13848 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13849 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13850 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13851 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13852 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13853 final double[] bias1 = calibrator.getInitialBias();
13854 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13855 final double[] bias2 = new double[3];
13856 calibrator.getInitialBias(bias2);
13857 assertArrayEquals(bias1, bias2, 0.0);
13858 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13859 assertEquals(b1, ba);
13860 final Matrix b2 = new Matrix(3, 1);
13861 calibrator.getInitialBiasAsMatrix(b2);
13862 assertEquals(b1, b2);
13863 final Matrix ma1 = calibrator.getInitialMa();
13864 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13865 final Matrix ma2 = new Matrix(3, 3);
13866 calibrator.getInitialMa(ma2);
13867 assertEquals(ma1, ma2);
13868 assertSame(calibrator.getEcefPosition(), ecefPosition);
13869 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13870 final NEDPosition nedPosition1 = new NEDPosition();
13871 assertTrue(calibrator.getNedPosition(nedPosition1));
13872 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13873 assertNull(calibrator.getMeasurements());
13874 assertTrue(calibrator.isCommonAxisUsed());
13875 assertSame(calibrator.getListener(), this);
13876 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13877 assertFalse(calibrator.isReady());
13878 assertFalse(calibrator.isRunning());
13879 assertNull(calibrator.getEstimatedBiases());
13880 assertFalse(calibrator.getEstimatedBiases(null));
13881 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13882 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13883 assertNull(calibrator.getEstimatedBiasFx());
13884 assertNull(calibrator.getEstimatedBiasFy());
13885 assertNull(calibrator.getEstimatedBiasFz());
13886 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13887 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13888 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13889 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13890 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13891 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13892 assertNull(calibrator.getEstimatedMa());
13893 assertNull(calibrator.getEstimatedSx());
13894 assertNull(calibrator.getEstimatedSy());
13895 assertNull(calibrator.getEstimatedSz());
13896 assertNull(calibrator.getEstimatedMxy());
13897 assertNull(calibrator.getEstimatedMxz());
13898 assertNull(calibrator.getEstimatedMyx());
13899 assertNull(calibrator.getEstimatedMyz());
13900 assertNull(calibrator.getEstimatedMzx());
13901 assertNull(calibrator.getEstimatedMzy());
13902 assertNull(calibrator.getEstimatedCovariance());
13903 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13904 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13905 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13906 assertNotNull(calibrator.getGroundTruthGravityNorm());
13907 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13908 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13909 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13910 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13911 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13912 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13913 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13914 }
13915
13916 @Test
13917 public void testConstructor116() throws WrongSizeException {
13918 final Collection<StandardDeviationBodyKinematics> measurements =
13919 Collections.emptyList();
13920
13921 final Matrix ba = generateBa();
13922 final double biasX = ba.getElementAtIndex(0);
13923 final double biasY = ba.getElementAtIndex(1);
13924 final double biasZ = ba.getElementAtIndex(2);
13925
13926 final Acceleration bx = new Acceleration(biasX,
13927 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13928 final Acceleration by = new Acceleration(biasY,
13929 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13930 final Acceleration bz = new Acceleration(biasZ,
13931 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13932
13933 final Matrix ma = generateMaCommonAxis();
13934 final double sx = ma.getElementAt(0, 0);
13935 final double sy = ma.getElementAt(1, 1);
13936 final double sz = ma.getElementAt(2, 2);
13937
13938 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13939 final double latitude = Math.toRadians(
13940 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13941 final double longitude = Math.toRadians(
13942 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13943 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13944 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13945 final NEDVelocity nedVelocity = new NEDVelocity();
13946 final ECEFPosition ecefPosition = new ECEFPosition();
13947 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13948 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13949 ecefPosition, ecefVelocity);
13950
13951 final KnownPositionAccelerometerCalibrator calibrator =
13952 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13953 true, bx, by, bz, sx, sy, sz);
13954
13955
13956 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13957 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13958 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13959 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13960 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13961 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13962 final Acceleration bx2 = new Acceleration(0.0,
13963 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13964 calibrator.getInitialBiasXAsAcceleration(bx2);
13965 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13966 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13967 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13968 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13969 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13970 final Acceleration by2 = new Acceleration(0.0,
13971 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13972 calibrator.getInitialBiasYAsAcceleration(by2);
13973 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13974 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13975 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13976 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13977 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13978 final Acceleration bz2 = new Acceleration(0.0,
13979 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13980 calibrator.getInitialBiasZAsAcceleration(bz2);
13981 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13982 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13983 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13984 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13985 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13986 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13987 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13988 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13989 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13990 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13991 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13992 final double[] bias1 = calibrator.getInitialBias();
13993 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13994 final double[] bias2 = new double[3];
13995 calibrator.getInitialBias(bias2);
13996 assertArrayEquals(bias1, bias2, 0.0);
13997 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13998 assertEquals(b1, ba);
13999 final Matrix b2 = new Matrix(3, 1);
14000 calibrator.getInitialBiasAsMatrix(b2);
14001 assertEquals(b1, b2);
14002 final Matrix ma1 = calibrator.getInitialMa();
14003 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14004 final Matrix ma2 = new Matrix(3, 3);
14005 calibrator.getInitialMa(ma2);
14006 assertEquals(ma1, ma2);
14007 assertSame(calibrator.getEcefPosition(), ecefPosition);
14008 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14009 final NEDPosition nedPosition1 = new NEDPosition();
14010 assertTrue(calibrator.getNedPosition(nedPosition1));
14011 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14012 assertSame(calibrator.getMeasurements(), measurements);
14013 assertTrue(calibrator.isCommonAxisUsed());
14014 assertNull(calibrator.getListener());
14015 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14016 assertFalse(calibrator.isReady());
14017 assertFalse(calibrator.isRunning());
14018 assertNull(calibrator.getEstimatedBiases());
14019 assertFalse(calibrator.getEstimatedBiases(null));
14020 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14021 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14022 assertNull(calibrator.getEstimatedBiasFx());
14023 assertNull(calibrator.getEstimatedBiasFy());
14024 assertNull(calibrator.getEstimatedBiasFz());
14025 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14026 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14027 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14028 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14029 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14030 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14031 assertNull(calibrator.getEstimatedMa());
14032 assertNull(calibrator.getEstimatedSx());
14033 assertNull(calibrator.getEstimatedSy());
14034 assertNull(calibrator.getEstimatedSz());
14035 assertNull(calibrator.getEstimatedMxy());
14036 assertNull(calibrator.getEstimatedMxz());
14037 assertNull(calibrator.getEstimatedMyx());
14038 assertNull(calibrator.getEstimatedMyz());
14039 assertNull(calibrator.getEstimatedMzx());
14040 assertNull(calibrator.getEstimatedMzy());
14041 assertNull(calibrator.getEstimatedCovariance());
14042 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14043 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14044 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14045 assertNotNull(calibrator.getGroundTruthGravityNorm());
14046 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14047 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14048 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14049 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14050 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14051 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14052 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14053 }
14054
14055 @Test
14056 public void testConstructor117() throws WrongSizeException {
14057 final Collection<StandardDeviationBodyKinematics> measurements =
14058 Collections.emptyList();
14059
14060 final Matrix ba = generateBa();
14061 final double biasX = ba.getElementAtIndex(0);
14062 final double biasY = ba.getElementAtIndex(1);
14063 final double biasZ = ba.getElementAtIndex(2);
14064
14065 final Acceleration bx = new Acceleration(biasX,
14066 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14067 final Acceleration by = new Acceleration(biasY,
14068 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14069 final Acceleration bz = new Acceleration(biasZ,
14070 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14071
14072 final Matrix ma = generateMaCommonAxis();
14073 final double sx = ma.getElementAt(0, 0);
14074 final double sy = ma.getElementAt(1, 1);
14075 final double sz = ma.getElementAt(2, 2);
14076
14077 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14078 final double latitude = Math.toRadians(
14079 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14080 final double longitude = Math.toRadians(
14081 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14082 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14083 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14084 final NEDVelocity nedVelocity = new NEDVelocity();
14085 final ECEFPosition ecefPosition = new ECEFPosition();
14086 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14087 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14088 ecefPosition, ecefVelocity);
14089
14090 final KnownPositionAccelerometerCalibrator calibrator =
14091 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14092 true, bx, by, bz, sx, sy, sz, this);
14093
14094
14095 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14096 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14097 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14098 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14099 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14100 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14101 final Acceleration bx2 = new Acceleration(0.0,
14102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14103 calibrator.getInitialBiasXAsAcceleration(bx2);
14104 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14105 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14106 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14107 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14108 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14109 final Acceleration by2 = new Acceleration(0.0,
14110 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14111 calibrator.getInitialBiasYAsAcceleration(by2);
14112 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14113 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14114 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14115 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14116 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14117 final Acceleration bz2 = new Acceleration(0.0,
14118 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14119 calibrator.getInitialBiasZAsAcceleration(bz2);
14120 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14121 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14122 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14123 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14124 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14125 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14126 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14127 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14128 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14129 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14130 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14131 final double[] bias1 = calibrator.getInitialBias();
14132 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14133 final double[] bias2 = new double[3];
14134 calibrator.getInitialBias(bias2);
14135 assertArrayEquals(bias1, bias2, 0.0);
14136 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14137 assertEquals(b1, ba);
14138 final Matrix b2 = new Matrix(3, 1);
14139 calibrator.getInitialBiasAsMatrix(b2);
14140 assertEquals(b1, b2);
14141 final Matrix ma1 = calibrator.getInitialMa();
14142 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14143 final Matrix ma2 = new Matrix(3, 3);
14144 calibrator.getInitialMa(ma2);
14145 assertEquals(ma1, ma2);
14146 assertSame(calibrator.getEcefPosition(), ecefPosition);
14147 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14148 final NEDPosition nedPosition1 = new NEDPosition();
14149 assertTrue(calibrator.getNedPosition(nedPosition1));
14150 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14151 assertSame(calibrator.getMeasurements(), measurements);
14152 assertTrue(calibrator.isCommonAxisUsed());
14153 assertSame(calibrator.getListener(), this);
14154 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14155 assertFalse(calibrator.isReady());
14156 assertFalse(calibrator.isRunning());
14157 assertNull(calibrator.getEstimatedBiases());
14158 assertFalse(calibrator.getEstimatedBiases(null));
14159 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14160 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14161 assertNull(calibrator.getEstimatedBiasFx());
14162 assertNull(calibrator.getEstimatedBiasFy());
14163 assertNull(calibrator.getEstimatedBiasFz());
14164 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14165 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14166 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14167 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14168 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14169 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14170 assertNull(calibrator.getEstimatedMa());
14171 assertNull(calibrator.getEstimatedSx());
14172 assertNull(calibrator.getEstimatedSy());
14173 assertNull(calibrator.getEstimatedSz());
14174 assertNull(calibrator.getEstimatedMxy());
14175 assertNull(calibrator.getEstimatedMxz());
14176 assertNull(calibrator.getEstimatedMyx());
14177 assertNull(calibrator.getEstimatedMyz());
14178 assertNull(calibrator.getEstimatedMzx());
14179 assertNull(calibrator.getEstimatedMzy());
14180 assertNull(calibrator.getEstimatedCovariance());
14181 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14182 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14183 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14184 assertNotNull(calibrator.getGroundTruthGravityNorm());
14185 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14186 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14187 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14188 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14189 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14190 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14191 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14192 }
14193
14194 @Test
14195 public void testConstructor118() throws WrongSizeException {
14196 final Matrix ba = generateBa();
14197 final double biasX = ba.getElementAtIndex(0);
14198 final double biasY = ba.getElementAtIndex(1);
14199 final double biasZ = ba.getElementAtIndex(2);
14200
14201 final Matrix ma = generateMaCommonAxis();
14202 final double sx = ma.getElementAt(0, 0);
14203 final double sy = ma.getElementAt(1, 1);
14204 final double sz = ma.getElementAt(2, 2);
14205 final double mxy = ma.getElementAt(0, 1);
14206 final double mxz = ma.getElementAt(0, 2);
14207 final double myx = ma.getElementAt(1, 0);
14208 final double myz = ma.getElementAt(1, 2);
14209 final double mzx = ma.getElementAt(2, 0);
14210 final double mzy = ma.getElementAt(2, 1);
14211
14212 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14213 final double latitude = Math.toRadians(
14214 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14215 final double longitude = Math.toRadians(
14216 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14217 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14218 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14219 final NEDVelocity nedVelocity = new NEDVelocity();
14220 final ECEFPosition ecefPosition = new ECEFPosition();
14221 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14222 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14223 ecefPosition, ecefVelocity);
14224
14225 final KnownPositionAccelerometerCalibrator calibrator =
14226 new KnownPositionAccelerometerCalibrator(ecefPosition,
14227 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14228 myx, myz, mzx, mzy);
14229
14230
14231 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14232 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14233 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14234 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14235 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14236 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14237 final Acceleration bx2 = new Acceleration(0.0,
14238 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14239 calibrator.getInitialBiasXAsAcceleration(bx2);
14240 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14241 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14242 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14243 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14244 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14245 final Acceleration by2 = new Acceleration(0.0,
14246 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14247 calibrator.getInitialBiasYAsAcceleration(by2);
14248 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14249 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14250 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14251 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14252 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14253 final Acceleration bz2 = new Acceleration(0.0,
14254 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14255 calibrator.getInitialBiasZAsAcceleration(bz2);
14256 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14257 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14258 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14259 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14260 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14261 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14262 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14263 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14264 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14265 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14266 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14267 final double[] bias1 = calibrator.getInitialBias();
14268 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14269 final double[] bias2 = new double[3];
14270 calibrator.getInitialBias(bias2);
14271 assertArrayEquals(bias1, bias2, 0.0);
14272 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14273 assertEquals(b1, ba);
14274 final Matrix b2 = new Matrix(3, 1);
14275 calibrator.getInitialBiasAsMatrix(b2);
14276 assertEquals(b1, b2);
14277 final Matrix ma1 = new Matrix(3, 3);
14278 ma1.setSubmatrix(0, 0,
14279 2, 2,
14280 new double[]{sx, myx, mzx,
14281 mxy, sy, mzy,
14282 mxz, myz, sz});
14283 assertEquals(calibrator.getInitialMa(), ma1);
14284 final Matrix ma2 = new Matrix(3, 3);
14285 calibrator.getInitialMa(ma2);
14286 assertEquals(ma1, ma2);
14287 assertSame(calibrator.getEcefPosition(), ecefPosition);
14288 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14289 final NEDPosition nedPosition1 = new NEDPosition();
14290 assertTrue(calibrator.getNedPosition(nedPosition1));
14291 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14292 assertNull(calibrator.getMeasurements());
14293 assertFalse(calibrator.isCommonAxisUsed());
14294 assertNull(calibrator.getListener());
14295 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14296 assertFalse(calibrator.isReady());
14297 assertFalse(calibrator.isRunning());
14298 assertNull(calibrator.getEstimatedBiases());
14299 assertFalse(calibrator.getEstimatedBiases(null));
14300 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14301 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14302 assertNull(calibrator.getEstimatedBiasFx());
14303 assertNull(calibrator.getEstimatedBiasFy());
14304 assertNull(calibrator.getEstimatedBiasFz());
14305 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14306 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14307 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14308 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14309 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14310 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14311 assertNull(calibrator.getEstimatedMa());
14312 assertNull(calibrator.getEstimatedSx());
14313 assertNull(calibrator.getEstimatedSy());
14314 assertNull(calibrator.getEstimatedSz());
14315 assertNull(calibrator.getEstimatedMxy());
14316 assertNull(calibrator.getEstimatedMxz());
14317 assertNull(calibrator.getEstimatedMyx());
14318 assertNull(calibrator.getEstimatedMyz());
14319 assertNull(calibrator.getEstimatedMzx());
14320 assertNull(calibrator.getEstimatedMzy());
14321 assertNull(calibrator.getEstimatedCovariance());
14322 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14323 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14324 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14325 assertNotNull(calibrator.getGroundTruthGravityNorm());
14326 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14327 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14328 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14329 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14330 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14331 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14332 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14333 }
14334
14335 @Test
14336 public void testConstructor119() throws WrongSizeException {
14337 final Collection<StandardDeviationBodyKinematics> measurements =
14338 Collections.emptyList();
14339
14340 final Matrix ba = generateBa();
14341 final double biasX = ba.getElementAtIndex(0);
14342 final double biasY = ba.getElementAtIndex(1);
14343 final double biasZ = ba.getElementAtIndex(2);
14344
14345 final Matrix ma = generateMaCommonAxis();
14346 final double sx = ma.getElementAt(0, 0);
14347 final double sy = ma.getElementAt(1, 1);
14348 final double sz = ma.getElementAt(2, 2);
14349 final double mxy = ma.getElementAt(0, 1);
14350 final double mxz = ma.getElementAt(0, 2);
14351 final double myx = ma.getElementAt(1, 0);
14352 final double myz = ma.getElementAt(1, 2);
14353 final double mzx = ma.getElementAt(2, 0);
14354 final double mzy = ma.getElementAt(2, 1);
14355
14356 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14357 final double latitude = Math.toRadians(
14358 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14359 final double longitude = Math.toRadians(
14360 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14361 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14362 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14363 final NEDVelocity nedVelocity = new NEDVelocity();
14364 final ECEFPosition ecefPosition = new ECEFPosition();
14365 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14366 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14367 ecefPosition, ecefVelocity);
14368
14369 final KnownPositionAccelerometerCalibrator calibrator =
14370 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14371 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14372 myx, myz, mzx, mzy);
14373
14374
14375 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14376 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14377 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14378 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14379 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14380 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14381 final Acceleration bx2 = new Acceleration(0.0,
14382 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14383 calibrator.getInitialBiasXAsAcceleration(bx2);
14384 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14385 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14386 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14387 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14388 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14389 final Acceleration by2 = new Acceleration(0.0,
14390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14391 calibrator.getInitialBiasYAsAcceleration(by2);
14392 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14393 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14394 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14395 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14396 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14397 final Acceleration bz2 = new Acceleration(0.0,
14398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14399 calibrator.getInitialBiasZAsAcceleration(bz2);
14400 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14401 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14402 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14403 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14404 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14405 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14406 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14407 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14408 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14409 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14410 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14411 final double[] bias1 = calibrator.getInitialBias();
14412 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14413 final double[] bias2 = new double[3];
14414 calibrator.getInitialBias(bias2);
14415 assertArrayEquals(bias1, bias2, 0.0);
14416 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14417 assertEquals(b1, ba);
14418 final Matrix b2 = new Matrix(3, 1);
14419 calibrator.getInitialBiasAsMatrix(b2);
14420 assertEquals(b1, b2);
14421 final Matrix ma1 = new Matrix(3, 3);
14422 ma1.setSubmatrix(0, 0,
14423 2, 2,
14424 new double[]{sx, myx, mzx,
14425 mxy, sy, mzy,
14426 mxz, myz, sz});
14427 assertEquals(calibrator.getInitialMa(), ma1);
14428 final Matrix ma2 = new Matrix(3, 3);
14429 calibrator.getInitialMa(ma2);
14430 assertEquals(ma1, ma2);
14431 assertSame(calibrator.getEcefPosition(), ecefPosition);
14432 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14433 final NEDPosition nedPosition1 = new NEDPosition();
14434 assertTrue(calibrator.getNedPosition(nedPosition1));
14435 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14436 assertSame(calibrator.getMeasurements(), measurements);
14437 assertFalse(calibrator.isCommonAxisUsed());
14438 assertNull(calibrator.getListener());
14439 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14440 assertFalse(calibrator.isReady());
14441 assertFalse(calibrator.isRunning());
14442 assertNull(calibrator.getEstimatedBiases());
14443 assertFalse(calibrator.getEstimatedBiases(null));
14444 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14445 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14446 assertNull(calibrator.getEstimatedBiasFx());
14447 assertNull(calibrator.getEstimatedBiasFy());
14448 assertNull(calibrator.getEstimatedBiasFz());
14449 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14450 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14451 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14452 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14453 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14454 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14455 assertNull(calibrator.getEstimatedMa());
14456 assertNull(calibrator.getEstimatedSx());
14457 assertNull(calibrator.getEstimatedSy());
14458 assertNull(calibrator.getEstimatedSz());
14459 assertNull(calibrator.getEstimatedMxy());
14460 assertNull(calibrator.getEstimatedMxz());
14461 assertNull(calibrator.getEstimatedMyx());
14462 assertNull(calibrator.getEstimatedMyz());
14463 assertNull(calibrator.getEstimatedMzx());
14464 assertNull(calibrator.getEstimatedMzy());
14465 assertNull(calibrator.getEstimatedCovariance());
14466 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14467 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14468 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14469 assertNotNull(calibrator.getGroundTruthGravityNorm());
14470 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14471 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14472 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14473 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14474 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14475 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14476 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14477 }
14478
14479 @Test
14480 public void testConstructor120() throws WrongSizeException {
14481 final Collection<StandardDeviationBodyKinematics> measurements =
14482 Collections.emptyList();
14483
14484 final Matrix ba = generateBa();
14485 final double biasX = ba.getElementAtIndex(0);
14486 final double biasY = ba.getElementAtIndex(1);
14487 final double biasZ = ba.getElementAtIndex(2);
14488
14489 final Matrix ma = generateMaCommonAxis();
14490 final double sx = ma.getElementAt(0, 0);
14491 final double sy = ma.getElementAt(1, 1);
14492 final double sz = ma.getElementAt(2, 2);
14493 final double mxy = ma.getElementAt(0, 1);
14494 final double mxz = ma.getElementAt(0, 2);
14495 final double myx = ma.getElementAt(1, 0);
14496 final double myz = ma.getElementAt(1, 2);
14497 final double mzx = ma.getElementAt(2, 0);
14498 final double mzy = ma.getElementAt(2, 1);
14499
14500 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14501 final double latitude = Math.toRadians(
14502 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14503 final double longitude = Math.toRadians(
14504 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14505 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14506 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14507 final NEDVelocity nedVelocity = new NEDVelocity();
14508 final ECEFPosition ecefPosition = new ECEFPosition();
14509 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14510 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14511 ecefPosition, ecefVelocity);
14512
14513 final KnownPositionAccelerometerCalibrator calibrator =
14514 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14515 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14516 myx, myz, mzx, mzy, this);
14517
14518
14519 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14520 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14521 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14522 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14523 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14524 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14525 final Acceleration bx2 = new Acceleration(0.0,
14526 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14527 calibrator.getInitialBiasXAsAcceleration(bx2);
14528 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14529 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14530 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14531 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14532 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14533 final Acceleration by2 = new Acceleration(0.0,
14534 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14535 calibrator.getInitialBiasYAsAcceleration(by2);
14536 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14537 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14538 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14539 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14540 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14541 final Acceleration bz2 = new Acceleration(0.0,
14542 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14543 calibrator.getInitialBiasZAsAcceleration(bz2);
14544 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14545 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14546 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14547 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14548 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14549 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14550 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14551 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14552 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14553 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14554 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14555 final double[] bias1 = calibrator.getInitialBias();
14556 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14557 final double[] bias2 = new double[3];
14558 calibrator.getInitialBias(bias2);
14559 assertArrayEquals(bias1, bias2, 0.0);
14560 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14561 assertEquals(b1, ba);
14562 final Matrix b2 = new Matrix(3, 1);
14563 calibrator.getInitialBiasAsMatrix(b2);
14564 assertEquals(b1, b2);
14565 final Matrix ma1 = new Matrix(3, 3);
14566 ma1.setSubmatrix(0, 0,
14567 2, 2,
14568 new double[]{sx, myx, mzx,
14569 mxy, sy, mzy,
14570 mxz, myz, sz});
14571 assertEquals(calibrator.getInitialMa(), ma1);
14572 final Matrix ma2 = new Matrix(3, 3);
14573 calibrator.getInitialMa(ma2);
14574 assertEquals(ma1, ma2);
14575 assertSame(calibrator.getEcefPosition(), ecefPosition);
14576 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14577 final NEDPosition nedPosition1 = new NEDPosition();
14578 assertTrue(calibrator.getNedPosition(nedPosition1));
14579 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14580 assertSame(calibrator.getMeasurements(), measurements);
14581 assertFalse(calibrator.isCommonAxisUsed());
14582 assertSame(calibrator.getListener(), this);
14583 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14584 assertFalse(calibrator.isReady());
14585 assertFalse(calibrator.isRunning());
14586 assertNull(calibrator.getEstimatedBiases());
14587 assertFalse(calibrator.getEstimatedBiases(null));
14588 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14589 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14590 assertNull(calibrator.getEstimatedBiasFx());
14591 assertNull(calibrator.getEstimatedBiasFy());
14592 assertNull(calibrator.getEstimatedBiasFz());
14593 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14594 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14595 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14596 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14597 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14598 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14599 assertNull(calibrator.getEstimatedMa());
14600 assertNull(calibrator.getEstimatedSx());
14601 assertNull(calibrator.getEstimatedSy());
14602 assertNull(calibrator.getEstimatedSz());
14603 assertNull(calibrator.getEstimatedMxy());
14604 assertNull(calibrator.getEstimatedMxz());
14605 assertNull(calibrator.getEstimatedMyx());
14606 assertNull(calibrator.getEstimatedMyz());
14607 assertNull(calibrator.getEstimatedMzx());
14608 assertNull(calibrator.getEstimatedMzy());
14609 assertNull(calibrator.getEstimatedCovariance());
14610 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14611 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14612 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14613 assertNotNull(calibrator.getGroundTruthGravityNorm());
14614 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14615 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14616 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14617 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14618 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14619 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14620 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14621 }
14622
14623 @Test
14624 public void testConstructor121() throws WrongSizeException {
14625 final Matrix ba = generateBa();
14626 final double biasX = ba.getElementAtIndex(0);
14627 final double biasY = ba.getElementAtIndex(1);
14628 final double biasZ = ba.getElementAtIndex(2);
14629
14630 final Matrix ma = generateMaCommonAxis();
14631 final double sx = ma.getElementAt(0, 0);
14632 final double sy = ma.getElementAt(1, 1);
14633 final double sz = ma.getElementAt(2, 2);
14634 final double mxy = ma.getElementAt(0, 1);
14635 final double mxz = ma.getElementAt(0, 2);
14636 final double myx = ma.getElementAt(1, 0);
14637 final double myz = ma.getElementAt(1, 2);
14638 final double mzx = ma.getElementAt(2, 0);
14639 final double mzy = ma.getElementAt(2, 1);
14640
14641 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14642 final double latitude = Math.toRadians(
14643 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14644 final double longitude = Math.toRadians(
14645 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14646 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14647 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14648 final NEDVelocity nedVelocity = new NEDVelocity();
14649 final ECEFPosition ecefPosition = new ECEFPosition();
14650 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14651 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14652 ecefPosition, ecefVelocity);
14653
14654 final KnownPositionAccelerometerCalibrator calibrator =
14655 new KnownPositionAccelerometerCalibrator(ecefPosition,
14656 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14657 myx, myz, mzx, mzy);
14658
14659
14660 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14661 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14662 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14663 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14664 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14665 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14666 final Acceleration bx2 = new Acceleration(0.0,
14667 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14668 calibrator.getInitialBiasXAsAcceleration(bx2);
14669 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14670 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14671 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14672 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14673 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14674 final Acceleration by2 = new Acceleration(0.0,
14675 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14676 calibrator.getInitialBiasYAsAcceleration(by2);
14677 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14678 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14679 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14680 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14681 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14682 final Acceleration bz2 = new Acceleration(0.0,
14683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14684 calibrator.getInitialBiasZAsAcceleration(bz2);
14685 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14686 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14687 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14688 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14689 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14690 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14691 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14692 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14693 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14694 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14695 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14696 final double[] bias1 = calibrator.getInitialBias();
14697 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14698 final double[] bias2 = new double[3];
14699 calibrator.getInitialBias(bias2);
14700 assertArrayEquals(bias1, bias2, 0.0);
14701 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14702 assertEquals(b1, ba);
14703 final Matrix b2 = new Matrix(3, 1);
14704 calibrator.getInitialBiasAsMatrix(b2);
14705 assertEquals(b1, b2);
14706 final Matrix ma1 = new Matrix(3, 3);
14707 ma1.setSubmatrix(0, 0,
14708 2, 2,
14709 new double[]{sx, myx, mzx,
14710 mxy, sy, mzy,
14711 mxz, myz, sz});
14712 assertEquals(calibrator.getInitialMa(), ma1);
14713 final Matrix ma2 = new Matrix(3, 3);
14714 calibrator.getInitialMa(ma2);
14715 assertEquals(ma1, ma2);
14716 assertSame(calibrator.getEcefPosition(), ecefPosition);
14717 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14718 final NEDPosition nedPosition1 = new NEDPosition();
14719 assertTrue(calibrator.getNedPosition(nedPosition1));
14720 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14721 assertNull(calibrator.getMeasurements());
14722 assertTrue(calibrator.isCommonAxisUsed());
14723 assertNull(calibrator.getListener());
14724 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14725 assertFalse(calibrator.isReady());
14726 assertFalse(calibrator.isRunning());
14727 assertNull(calibrator.getEstimatedBiases());
14728 assertFalse(calibrator.getEstimatedBiases(null));
14729 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14730 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14731 assertNull(calibrator.getEstimatedBiasFx());
14732 assertNull(calibrator.getEstimatedBiasFy());
14733 assertNull(calibrator.getEstimatedBiasFz());
14734 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14735 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14736 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14737 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14738 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14739 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14740 assertNull(calibrator.getEstimatedMa());
14741 assertNull(calibrator.getEstimatedSx());
14742 assertNull(calibrator.getEstimatedSy());
14743 assertNull(calibrator.getEstimatedSz());
14744 assertNull(calibrator.getEstimatedMxy());
14745 assertNull(calibrator.getEstimatedMxz());
14746 assertNull(calibrator.getEstimatedMyx());
14747 assertNull(calibrator.getEstimatedMyz());
14748 assertNull(calibrator.getEstimatedMzx());
14749 assertNull(calibrator.getEstimatedMzy());
14750 assertNull(calibrator.getEstimatedCovariance());
14751 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14752 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14753 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14754 assertNotNull(calibrator.getGroundTruthGravityNorm());
14755 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14756 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14757 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14758 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14759 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14760 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14761 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14762 }
14763
14764 @Test
14765 public void testConstructor122() throws WrongSizeException {
14766 final Matrix ba = generateBa();
14767 final double biasX = ba.getElementAtIndex(0);
14768 final double biasY = ba.getElementAtIndex(1);
14769 final double biasZ = ba.getElementAtIndex(2);
14770
14771 final Matrix ma = generateMaCommonAxis();
14772 final double sx = ma.getElementAt(0, 0);
14773 final double sy = ma.getElementAt(1, 1);
14774 final double sz = ma.getElementAt(2, 2);
14775 final double mxy = ma.getElementAt(0, 1);
14776 final double mxz = ma.getElementAt(0, 2);
14777 final double myx = ma.getElementAt(1, 0);
14778 final double myz = ma.getElementAt(1, 2);
14779 final double mzx = ma.getElementAt(2, 0);
14780 final double mzy = ma.getElementAt(2, 1);
14781
14782 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14783 final double latitude = Math.toRadians(
14784 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14785 final double longitude = Math.toRadians(
14786 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14787 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14788 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14789 final NEDVelocity nedVelocity = new NEDVelocity();
14790 final ECEFPosition ecefPosition = new ECEFPosition();
14791 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14792 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14793 ecefPosition, ecefVelocity);
14794
14795 final KnownPositionAccelerometerCalibrator calibrator =
14796 new KnownPositionAccelerometerCalibrator(ecefPosition,
14797 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14798 myx, myz, mzx, mzy, this);
14799
14800
14801 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14802 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14803 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14804 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14805 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14806 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14807 final Acceleration bx2 = new Acceleration(0.0,
14808 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14809 calibrator.getInitialBiasXAsAcceleration(bx2);
14810 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14811 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14812 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14813 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14814 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14815 final Acceleration by2 = new Acceleration(0.0,
14816 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14817 calibrator.getInitialBiasYAsAcceleration(by2);
14818 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14819 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14820 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14821 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14822 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14823 final Acceleration bz2 = new Acceleration(0.0,
14824 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14825 calibrator.getInitialBiasZAsAcceleration(bz2);
14826 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14827 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14828 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14829 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14830 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14831 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14832 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14833 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14834 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14835 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14836 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14837 final double[] bias1 = calibrator.getInitialBias();
14838 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14839 final double[] bias2 = new double[3];
14840 calibrator.getInitialBias(bias2);
14841 assertArrayEquals(bias1, bias2, 0.0);
14842 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14843 assertEquals(b1, ba);
14844 final Matrix b2 = new Matrix(3, 1);
14845 calibrator.getInitialBiasAsMatrix(b2);
14846 assertEquals(b1, b2);
14847 final Matrix ma1 = new Matrix(3, 3);
14848 ma1.setSubmatrix(0, 0,
14849 2, 2,
14850 new double[]{sx, myx, mzx,
14851 mxy, sy, mzy,
14852 mxz, myz, sz});
14853 assertEquals(calibrator.getInitialMa(), ma1);
14854 final Matrix ma2 = new Matrix(3, 3);
14855 calibrator.getInitialMa(ma2);
14856 assertEquals(ma1, ma2);
14857 assertSame(calibrator.getEcefPosition(), ecefPosition);
14858 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14859 final NEDPosition nedPosition1 = new NEDPosition();
14860 assertTrue(calibrator.getNedPosition(nedPosition1));
14861 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14862 assertNull(calibrator.getMeasurements());
14863 assertTrue(calibrator.isCommonAxisUsed());
14864 assertSame(calibrator.getListener(), this);
14865 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14866 assertFalse(calibrator.isReady());
14867 assertFalse(calibrator.isRunning());
14868 assertNull(calibrator.getEstimatedBiases());
14869 assertFalse(calibrator.getEstimatedBiases(null));
14870 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14871 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14872 assertNull(calibrator.getEstimatedBiasFx());
14873 assertNull(calibrator.getEstimatedBiasFy());
14874 assertNull(calibrator.getEstimatedBiasFz());
14875 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14876 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14877 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14878 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14879 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14880 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14881 assertNull(calibrator.getEstimatedMa());
14882 assertNull(calibrator.getEstimatedSx());
14883 assertNull(calibrator.getEstimatedSy());
14884 assertNull(calibrator.getEstimatedSz());
14885 assertNull(calibrator.getEstimatedMxy());
14886 assertNull(calibrator.getEstimatedMxz());
14887 assertNull(calibrator.getEstimatedMyx());
14888 assertNull(calibrator.getEstimatedMyz());
14889 assertNull(calibrator.getEstimatedMzx());
14890 assertNull(calibrator.getEstimatedMzy());
14891 assertNull(calibrator.getEstimatedCovariance());
14892 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14893 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14894 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14895 assertNotNull(calibrator.getGroundTruthGravityNorm());
14896 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14897 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14898 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14899 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14900 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14901 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14902 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14903 }
14904
14905 @Test
14906 public void testConstructor123() throws WrongSizeException {
14907 final Collection<StandardDeviationBodyKinematics> measurements =
14908 Collections.emptyList();
14909
14910 final Matrix ba = generateBa();
14911 final double biasX = ba.getElementAtIndex(0);
14912 final double biasY = ba.getElementAtIndex(1);
14913 final double biasZ = ba.getElementAtIndex(2);
14914
14915 final Matrix ma = generateMaCommonAxis();
14916 final double sx = ma.getElementAt(0, 0);
14917 final double sy = ma.getElementAt(1, 1);
14918 final double sz = ma.getElementAt(2, 2);
14919 final double mxy = ma.getElementAt(0, 1);
14920 final double mxz = ma.getElementAt(0, 2);
14921 final double myx = ma.getElementAt(1, 0);
14922 final double myz = ma.getElementAt(1, 2);
14923 final double mzx = ma.getElementAt(2, 0);
14924 final double mzy = ma.getElementAt(2, 1);
14925
14926 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14927 final double latitude = Math.toRadians(
14928 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14929 final double longitude = Math.toRadians(
14930 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14931 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14932 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14933 final NEDVelocity nedVelocity = new NEDVelocity();
14934 final ECEFPosition ecefPosition = new ECEFPosition();
14935 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14936 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14937 ecefPosition, ecefVelocity);
14938
14939 final KnownPositionAccelerometerCalibrator calibrator =
14940 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14941 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14942 myx, myz, mzx, mzy);
14943
14944
14945 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14946 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14947 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14948 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14949 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14950 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14951 final Acceleration bx2 = new Acceleration(0.0,
14952 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14953 calibrator.getInitialBiasXAsAcceleration(bx2);
14954 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14955 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14956 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14957 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14958 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14959 final Acceleration by2 = new Acceleration(0.0,
14960 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14961 calibrator.getInitialBiasYAsAcceleration(by2);
14962 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14963 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14964 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14965 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14966 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14967 final Acceleration bz2 = new Acceleration(0.0,
14968 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14969 calibrator.getInitialBiasZAsAcceleration(bz2);
14970 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14971 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14972 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14973 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14974 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14975 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14976 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14977 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14978 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14979 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14980 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14981 final double[] bias1 = calibrator.getInitialBias();
14982 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14983 final double[] bias2 = new double[3];
14984 calibrator.getInitialBias(bias2);
14985 assertArrayEquals(bias1, bias2, 0.0);
14986 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14987 assertEquals(b1, ba);
14988 final Matrix b2 = new Matrix(3, 1);
14989 calibrator.getInitialBiasAsMatrix(b2);
14990 assertEquals(b1, b2);
14991 final Matrix ma1 = new Matrix(3, 3);
14992 ma1.setSubmatrix(0, 0,
14993 2, 2,
14994 new double[]{sx, myx, mzx,
14995 mxy, sy, mzy,
14996 mxz, myz, sz});
14997 assertEquals(calibrator.getInitialMa(), ma1);
14998 final Matrix ma2 = new Matrix(3, 3);
14999 calibrator.getInitialMa(ma2);
15000 assertEquals(ma1, ma2);
15001 assertSame(calibrator.getEcefPosition(), ecefPosition);
15002 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15003 final NEDPosition nedPosition1 = new NEDPosition();
15004 assertTrue(calibrator.getNedPosition(nedPosition1));
15005 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15006 assertSame(calibrator.getMeasurements(), measurements);
15007 assertTrue(calibrator.isCommonAxisUsed());
15008 assertNull(calibrator.getListener());
15009 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15010 assertFalse(calibrator.isReady());
15011 assertFalse(calibrator.isRunning());
15012 assertNull(calibrator.getEstimatedBiases());
15013 assertFalse(calibrator.getEstimatedBiases(null));
15014 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15015 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15016 assertNull(calibrator.getEstimatedBiasFx());
15017 assertNull(calibrator.getEstimatedBiasFy());
15018 assertNull(calibrator.getEstimatedBiasFz());
15019 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15020 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15021 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15022 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15023 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15024 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15025 assertNull(calibrator.getEstimatedMa());
15026 assertNull(calibrator.getEstimatedSx());
15027 assertNull(calibrator.getEstimatedSy());
15028 assertNull(calibrator.getEstimatedSz());
15029 assertNull(calibrator.getEstimatedMxy());
15030 assertNull(calibrator.getEstimatedMxz());
15031 assertNull(calibrator.getEstimatedMyx());
15032 assertNull(calibrator.getEstimatedMyz());
15033 assertNull(calibrator.getEstimatedMzx());
15034 assertNull(calibrator.getEstimatedMzy());
15035 assertNull(calibrator.getEstimatedCovariance());
15036 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15037 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15038 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15039 assertNotNull(calibrator.getGroundTruthGravityNorm());
15040 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15041 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15042 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15043 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15044 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15045 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15046 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15047 }
15048
15049 @Test
15050 public void testConstructor124() throws WrongSizeException {
15051 final Collection<StandardDeviationBodyKinematics> measurements =
15052 Collections.emptyList();
15053
15054 final Matrix ba = generateBa();
15055 final double biasX = ba.getElementAtIndex(0);
15056 final double biasY = ba.getElementAtIndex(1);
15057 final double biasZ = ba.getElementAtIndex(2);
15058
15059 final Matrix ma = generateMaCommonAxis();
15060 final double sx = ma.getElementAt(0, 0);
15061 final double sy = ma.getElementAt(1, 1);
15062 final double sz = ma.getElementAt(2, 2);
15063 final double mxy = ma.getElementAt(0, 1);
15064 final double mxz = ma.getElementAt(0, 2);
15065 final double myx = ma.getElementAt(1, 0);
15066 final double myz = ma.getElementAt(1, 2);
15067 final double mzx = ma.getElementAt(2, 0);
15068 final double mzy = ma.getElementAt(2, 1);
15069
15070 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15071 final double latitude = Math.toRadians(
15072 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15073 final double longitude = Math.toRadians(
15074 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15075 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15076 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15077 final NEDVelocity nedVelocity = new NEDVelocity();
15078 final ECEFPosition ecefPosition = new ECEFPosition();
15079 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15080 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15081 ecefPosition, ecefVelocity);
15082
15083 final KnownPositionAccelerometerCalibrator calibrator =
15084 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15085 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
15086 myx, myz, mzx, mzy, this);
15087
15088
15089 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15090 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15091 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15092 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15093 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15094 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15095 final Acceleration bx2 = new Acceleration(0.0,
15096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15097 calibrator.getInitialBiasXAsAcceleration(bx2);
15098 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15099 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15100 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15101 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15102 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15103 final Acceleration by2 = new Acceleration(0.0,
15104 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15105 calibrator.getInitialBiasYAsAcceleration(by2);
15106 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15107 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15108 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15109 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15110 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15111 final Acceleration bz2 = new Acceleration(0.0,
15112 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15113 calibrator.getInitialBiasZAsAcceleration(bz2);
15114 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15115 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15116 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15117 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15118 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15119 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15120 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15121 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15122 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15123 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15124 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15125 final double[] bias1 = calibrator.getInitialBias();
15126 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15127 final double[] bias2 = new double[3];
15128 calibrator.getInitialBias(bias2);
15129 assertArrayEquals(bias1, bias2, 0.0);
15130 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15131 assertEquals(b1, ba);
15132 final Matrix b2 = new Matrix(3, 1);
15133 calibrator.getInitialBiasAsMatrix(b2);
15134 assertEquals(b1, b2);
15135 final Matrix ma1 = new Matrix(3, 3);
15136 ma1.setSubmatrix(0, 0,
15137 2, 2,
15138 new double[]{sx, myx, mzx,
15139 mxy, sy, mzy,
15140 mxz, myz, sz});
15141 assertEquals(calibrator.getInitialMa(), ma1);
15142 final Matrix ma2 = new Matrix(3, 3);
15143 calibrator.getInitialMa(ma2);
15144 assertEquals(ma1, ma2);
15145 assertSame(calibrator.getEcefPosition(), ecefPosition);
15146 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15147 final NEDPosition nedPosition1 = new NEDPosition();
15148 assertTrue(calibrator.getNedPosition(nedPosition1));
15149 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15150 assertSame(calibrator.getMeasurements(), measurements);
15151 assertTrue(calibrator.isCommonAxisUsed());
15152 assertSame(calibrator.getListener(), this);
15153 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15154 assertFalse(calibrator.isReady());
15155 assertFalse(calibrator.isRunning());
15156 assertNull(calibrator.getEstimatedBiases());
15157 assertFalse(calibrator.getEstimatedBiases(null));
15158 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15159 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15160 assertNull(calibrator.getEstimatedBiasFx());
15161 assertNull(calibrator.getEstimatedBiasFy());
15162 assertNull(calibrator.getEstimatedBiasFz());
15163 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15164 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15165 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15166 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15167 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15168 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15169 assertNull(calibrator.getEstimatedMa());
15170 assertNull(calibrator.getEstimatedSx());
15171 assertNull(calibrator.getEstimatedSy());
15172 assertNull(calibrator.getEstimatedSz());
15173 assertNull(calibrator.getEstimatedMxy());
15174 assertNull(calibrator.getEstimatedMxz());
15175 assertNull(calibrator.getEstimatedMyx());
15176 assertNull(calibrator.getEstimatedMyz());
15177 assertNull(calibrator.getEstimatedMzx());
15178 assertNull(calibrator.getEstimatedMzy());
15179 assertNull(calibrator.getEstimatedCovariance());
15180 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15181 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15182 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15183 assertNotNull(calibrator.getGroundTruthGravityNorm());
15184 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15185 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15186 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15187 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15188 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15189 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15190 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15191 }
15192
15193 @Test
15194 public void testConstructor125() throws WrongSizeException {
15195 final Matrix ba = generateBa();
15196 final double biasX = ba.getElementAtIndex(0);
15197 final double biasY = ba.getElementAtIndex(1);
15198 final double biasZ = ba.getElementAtIndex(2);
15199
15200 final Acceleration bx = new Acceleration(biasX,
15201 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15202 final Acceleration by = new Acceleration(biasY,
15203 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15204 final Acceleration bz = new Acceleration(biasZ,
15205 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15206
15207 final Matrix ma = generateMaCommonAxis();
15208 final double sx = ma.getElementAt(0, 0);
15209 final double sy = ma.getElementAt(1, 1);
15210 final double sz = ma.getElementAt(2, 2);
15211 final double mxy = ma.getElementAt(0, 1);
15212 final double mxz = ma.getElementAt(0, 2);
15213 final double myx = ma.getElementAt(1, 0);
15214 final double myz = ma.getElementAt(1, 2);
15215 final double mzx = ma.getElementAt(2, 0);
15216 final double mzy = ma.getElementAt(2, 1);
15217
15218 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15219 final double latitude = Math.toRadians(
15220 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15221 final double longitude = Math.toRadians(
15222 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15223 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15224 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15225 final NEDVelocity nedVelocity = new NEDVelocity();
15226 final ECEFPosition ecefPosition = new ECEFPosition();
15227 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15228 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15229 ecefPosition, ecefVelocity);
15230
15231 final KnownPositionAccelerometerCalibrator calibrator =
15232 new KnownPositionAccelerometerCalibrator(ecefPosition,
15233 bx, by, bz, sx, sy, sz, mxy, mxz,
15234 myx, myz, mzx, mzy);
15235
15236
15237 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15238 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15239 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15240 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15241 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15242 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15243 final Acceleration bx2 = new Acceleration(0.0,
15244 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15245 calibrator.getInitialBiasXAsAcceleration(bx2);
15246 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15247 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15248 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15249 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15250 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15251 final Acceleration by2 = new Acceleration(0.0,
15252 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15253 calibrator.getInitialBiasYAsAcceleration(by2);
15254 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15255 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15256 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15257 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15258 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15259 final Acceleration bz2 = new Acceleration(0.0,
15260 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15261 calibrator.getInitialBiasZAsAcceleration(bz2);
15262 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15263 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15264 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15265 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15266 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15267 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15268 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15269 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15270 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15271 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15272 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15273 final double[] bias1 = calibrator.getInitialBias();
15274 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15275 final double[] bias2 = new double[3];
15276 calibrator.getInitialBias(bias2);
15277 assertArrayEquals(bias1, bias2, 0.0);
15278 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15279 assertEquals(b1, ba);
15280 final Matrix b2 = new Matrix(3, 1);
15281 calibrator.getInitialBiasAsMatrix(b2);
15282 assertEquals(b1, b2);
15283 final Matrix ma1 = new Matrix(3, 3);
15284 ma1.setSubmatrix(0, 0,
15285 2, 2,
15286 new double[]{sx, myx, mzx,
15287 mxy, sy, mzy,
15288 mxz, myz, sz});
15289 assertEquals(calibrator.getInitialMa(), ma1);
15290 final Matrix ma2 = new Matrix(3, 3);
15291 calibrator.getInitialMa(ma2);
15292 assertEquals(ma1, ma2);
15293 assertSame(calibrator.getEcefPosition(), ecefPosition);
15294 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15295 final NEDPosition nedPosition1 = new NEDPosition();
15296 assertTrue(calibrator.getNedPosition(nedPosition1));
15297 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15298 assertNull(calibrator.getMeasurements());
15299 assertFalse(calibrator.isCommonAxisUsed());
15300 assertNull(calibrator.getListener());
15301 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15302 assertFalse(calibrator.isReady());
15303 assertFalse(calibrator.isRunning());
15304 assertNull(calibrator.getEstimatedBiases());
15305 assertFalse(calibrator.getEstimatedBiases(null));
15306 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15307 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15308 assertNull(calibrator.getEstimatedBiasFx());
15309 assertNull(calibrator.getEstimatedBiasFy());
15310 assertNull(calibrator.getEstimatedBiasFz());
15311 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15312 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15313 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15314 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15315 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15316 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15317 assertNull(calibrator.getEstimatedMa());
15318 assertNull(calibrator.getEstimatedSx());
15319 assertNull(calibrator.getEstimatedSy());
15320 assertNull(calibrator.getEstimatedSz());
15321 assertNull(calibrator.getEstimatedMxy());
15322 assertNull(calibrator.getEstimatedMxz());
15323 assertNull(calibrator.getEstimatedMyx());
15324 assertNull(calibrator.getEstimatedMyz());
15325 assertNull(calibrator.getEstimatedMzx());
15326 assertNull(calibrator.getEstimatedMzy());
15327 assertNull(calibrator.getEstimatedCovariance());
15328 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15329 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15330 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15331 assertNotNull(calibrator.getGroundTruthGravityNorm());
15332 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15333 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15334 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15335 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15336 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15337 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15338 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15339 }
15340
15341 @Test
15342 public void testConstructor126() throws WrongSizeException {
15343 final Matrix ba = generateBa();
15344 final double biasX = ba.getElementAtIndex(0);
15345 final double biasY = ba.getElementAtIndex(1);
15346 final double biasZ = ba.getElementAtIndex(2);
15347
15348 final Acceleration bx = new Acceleration(biasX,
15349 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15350 final Acceleration by = new Acceleration(biasY,
15351 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15352 final Acceleration bz = new Acceleration(biasZ,
15353 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15354
15355 final Matrix ma = generateMaCommonAxis();
15356 final double sx = ma.getElementAt(0, 0);
15357 final double sy = ma.getElementAt(1, 1);
15358 final double sz = ma.getElementAt(2, 2);
15359 final double mxy = ma.getElementAt(0, 1);
15360 final double mxz = ma.getElementAt(0, 2);
15361 final double myx = ma.getElementAt(1, 0);
15362 final double myz = ma.getElementAt(1, 2);
15363 final double mzx = ma.getElementAt(2, 0);
15364 final double mzy = ma.getElementAt(2, 1);
15365
15366 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15367 final double latitude = Math.toRadians(
15368 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15369 final double longitude = Math.toRadians(
15370 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15371 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15372 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15373 final NEDVelocity nedVelocity = new NEDVelocity();
15374 final ECEFPosition ecefPosition = new ECEFPosition();
15375 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15376 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15377 ecefPosition, ecefVelocity);
15378
15379 final KnownPositionAccelerometerCalibrator calibrator =
15380 new KnownPositionAccelerometerCalibrator(ecefPosition,
15381 bx, by, bz, sx, sy, sz, mxy, mxz,
15382 myx, myz, mzx, mzy, this);
15383
15384
15385 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15386 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15387 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15388 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15389 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15390 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15391 final Acceleration bx2 = new Acceleration(0.0,
15392 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15393 calibrator.getInitialBiasXAsAcceleration(bx2);
15394 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15395 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15396 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15397 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15398 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15399 final Acceleration by2 = new Acceleration(0.0,
15400 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15401 calibrator.getInitialBiasYAsAcceleration(by2);
15402 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15403 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15404 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15405 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15406 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15407 final Acceleration bz2 = new Acceleration(0.0,
15408 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15409 calibrator.getInitialBiasZAsAcceleration(bz2);
15410 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15411 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15412 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15413 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15414 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15415 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15416 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15417 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15418 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15419 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15420 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15421 final double[] bias1 = calibrator.getInitialBias();
15422 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15423 final double[] bias2 = new double[3];
15424 calibrator.getInitialBias(bias2);
15425 assertArrayEquals(bias1, bias2, 0.0);
15426 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15427 assertEquals(b1, ba);
15428 final Matrix b2 = new Matrix(3, 1);
15429 calibrator.getInitialBiasAsMatrix(b2);
15430 assertEquals(b1, b2);
15431 final Matrix ma1 = new Matrix(3, 3);
15432 ma1.setSubmatrix(0, 0,
15433 2, 2,
15434 new double[]{sx, myx, mzx,
15435 mxy, sy, mzy,
15436 mxz, myz, sz});
15437 assertEquals(calibrator.getInitialMa(), ma1);
15438 final Matrix ma2 = new Matrix(3, 3);
15439 calibrator.getInitialMa(ma2);
15440 assertEquals(ma1, ma2);
15441 assertSame(calibrator.getEcefPosition(), ecefPosition);
15442 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15443 final NEDPosition nedPosition1 = new NEDPosition();
15444 assertTrue(calibrator.getNedPosition(nedPosition1));
15445 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15446 assertNull(calibrator.getMeasurements());
15447 assertFalse(calibrator.isCommonAxisUsed());
15448 assertSame(calibrator.getListener(), this);
15449 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15450 assertFalse(calibrator.isReady());
15451 assertFalse(calibrator.isRunning());
15452 assertNull(calibrator.getEstimatedBiases());
15453 assertFalse(calibrator.getEstimatedBiases(null));
15454 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15455 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15456 assertNull(calibrator.getEstimatedBiasFx());
15457 assertNull(calibrator.getEstimatedBiasFy());
15458 assertNull(calibrator.getEstimatedBiasFz());
15459 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15460 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15461 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15462 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15463 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15464 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15465 assertNull(calibrator.getEstimatedMa());
15466 assertNull(calibrator.getEstimatedSx());
15467 assertNull(calibrator.getEstimatedSy());
15468 assertNull(calibrator.getEstimatedSz());
15469 assertNull(calibrator.getEstimatedMxy());
15470 assertNull(calibrator.getEstimatedMxz());
15471 assertNull(calibrator.getEstimatedMyx());
15472 assertNull(calibrator.getEstimatedMyz());
15473 assertNull(calibrator.getEstimatedMzx());
15474 assertNull(calibrator.getEstimatedMzy());
15475 assertNull(calibrator.getEstimatedCovariance());
15476 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15477 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15478 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15479 assertNotNull(calibrator.getGroundTruthGravityNorm());
15480 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15481 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15482 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15483 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15484 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15485 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15486 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15487 }
15488
15489 @Test
15490 public void testConstructor127() throws WrongSizeException {
15491 final Collection<StandardDeviationBodyKinematics> measurements =
15492 Collections.emptyList();
15493
15494 final Matrix ba = generateBa();
15495 final double biasX = ba.getElementAtIndex(0);
15496 final double biasY = ba.getElementAtIndex(1);
15497 final double biasZ = ba.getElementAtIndex(2);
15498
15499 final Acceleration bx = new Acceleration(biasX,
15500 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15501 final Acceleration by = new Acceleration(biasY,
15502 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15503 final Acceleration bz = new Acceleration(biasZ,
15504 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15505
15506 final Matrix ma = generateMaCommonAxis();
15507 final double sx = ma.getElementAt(0, 0);
15508 final double sy = ma.getElementAt(1, 1);
15509 final double sz = ma.getElementAt(2, 2);
15510 final double mxy = ma.getElementAt(0, 1);
15511 final double mxz = ma.getElementAt(0, 2);
15512 final double myx = ma.getElementAt(1, 0);
15513 final double myz = ma.getElementAt(1, 2);
15514 final double mzx = ma.getElementAt(2, 0);
15515 final double mzy = ma.getElementAt(2, 1);
15516
15517 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15518 final double latitude = Math.toRadians(
15519 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15520 final double longitude = Math.toRadians(
15521 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15522 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15523 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15524 final NEDVelocity nedVelocity = new NEDVelocity();
15525 final ECEFPosition ecefPosition = new ECEFPosition();
15526 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15527 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15528 ecefPosition, ecefVelocity);
15529
15530 final KnownPositionAccelerometerCalibrator calibrator =
15531 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15532 bx, by, bz, sx, sy, sz, mxy, mxz,
15533 myx, myz, mzx, mzy);
15534
15535
15536 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15537 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15538 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15539 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15540 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15541 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15542 final Acceleration bx2 = new Acceleration(0.0,
15543 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15544 calibrator.getInitialBiasXAsAcceleration(bx2);
15545 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15546 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15547 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15548 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15549 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15550 final Acceleration by2 = new Acceleration(0.0,
15551 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15552 calibrator.getInitialBiasYAsAcceleration(by2);
15553 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15554 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15555 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15556 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15557 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15558 final Acceleration bz2 = new Acceleration(0.0,
15559 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15560 calibrator.getInitialBiasZAsAcceleration(bz2);
15561 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15562 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15563 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15564 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15565 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15566 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15567 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15568 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15569 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15570 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15571 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15572 final double[] bias1 = calibrator.getInitialBias();
15573 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15574 final double[] bias2 = new double[3];
15575 calibrator.getInitialBias(bias2);
15576 assertArrayEquals(bias1, bias2, 0.0);
15577 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15578 assertEquals(b1, ba);
15579 final Matrix b2 = new Matrix(3, 1);
15580 calibrator.getInitialBiasAsMatrix(b2);
15581 assertEquals(b1, b2);
15582 final Matrix ma1 = new Matrix(3, 3);
15583 ma1.setSubmatrix(0, 0,
15584 2, 2,
15585 new double[]{sx, myx, mzx,
15586 mxy, sy, mzy,
15587 mxz, myz, sz});
15588 assertEquals(calibrator.getInitialMa(), ma1);
15589 final Matrix ma2 = new Matrix(3, 3);
15590 calibrator.getInitialMa(ma2);
15591 assertEquals(ma1, ma2);
15592 assertSame(calibrator.getEcefPosition(), ecefPosition);
15593 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15594 final NEDPosition nedPosition1 = new NEDPosition();
15595 assertTrue(calibrator.getNedPosition(nedPosition1));
15596 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15597 assertSame(calibrator.getMeasurements(), measurements);
15598 assertFalse(calibrator.isCommonAxisUsed());
15599 assertNull(calibrator.getListener());
15600 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15601 assertFalse(calibrator.isReady());
15602 assertFalse(calibrator.isRunning());
15603 assertNull(calibrator.getEstimatedBiases());
15604 assertFalse(calibrator.getEstimatedBiases(null));
15605 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15606 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15607 assertNull(calibrator.getEstimatedBiasFx());
15608 assertNull(calibrator.getEstimatedBiasFy());
15609 assertNull(calibrator.getEstimatedBiasFz());
15610 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15611 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15612 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15613 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15614 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15615 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15616 assertNull(calibrator.getEstimatedMa());
15617 assertNull(calibrator.getEstimatedSx());
15618 assertNull(calibrator.getEstimatedSy());
15619 assertNull(calibrator.getEstimatedSz());
15620 assertNull(calibrator.getEstimatedMxy());
15621 assertNull(calibrator.getEstimatedMxz());
15622 assertNull(calibrator.getEstimatedMyx());
15623 assertNull(calibrator.getEstimatedMyz());
15624 assertNull(calibrator.getEstimatedMzx());
15625 assertNull(calibrator.getEstimatedMzy());
15626 assertNull(calibrator.getEstimatedCovariance());
15627 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15628 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15629 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15630 assertNotNull(calibrator.getGroundTruthGravityNorm());
15631 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15632 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15633 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15634 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15635 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15636 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15637 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15638 }
15639
15640 @Test
15641 public void testConstructor128() throws WrongSizeException {
15642 final Collection<StandardDeviationBodyKinematics> measurements =
15643 Collections.emptyList();
15644
15645 final Matrix ba = generateBa();
15646 final double biasX = ba.getElementAtIndex(0);
15647 final double biasY = ba.getElementAtIndex(1);
15648 final double biasZ = ba.getElementAtIndex(2);
15649
15650 final Acceleration bx = new Acceleration(biasX,
15651 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15652 final Acceleration by = new Acceleration(biasY,
15653 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15654 final Acceleration bz = new Acceleration(biasZ,
15655 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15656
15657 final Matrix ma = generateMaCommonAxis();
15658 final double sx = ma.getElementAt(0, 0);
15659 final double sy = ma.getElementAt(1, 1);
15660 final double sz = ma.getElementAt(2, 2);
15661 final double mxy = ma.getElementAt(0, 1);
15662 final double mxz = ma.getElementAt(0, 2);
15663 final double myx = ma.getElementAt(1, 0);
15664 final double myz = ma.getElementAt(1, 2);
15665 final double mzx = ma.getElementAt(2, 0);
15666 final double mzy = ma.getElementAt(2, 1);
15667
15668 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15669 final double latitude = Math.toRadians(
15670 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15671 final double longitude = Math.toRadians(
15672 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15673 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15674 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15675 final NEDVelocity nedVelocity = new NEDVelocity();
15676 final ECEFPosition ecefPosition = new ECEFPosition();
15677 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15678 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15679 ecefPosition, ecefVelocity);
15680
15681 final KnownPositionAccelerometerCalibrator calibrator =
15682 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15683 bx, by, bz, sx, sy, sz, mxy, mxz,
15684 myx, myz, mzx, mzy, this);
15685
15686
15687 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15688 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15689 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15690 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15691 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15692 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15693 final Acceleration bx2 = new Acceleration(0.0,
15694 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15695 calibrator.getInitialBiasXAsAcceleration(bx2);
15696 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15697 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15698 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15699 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15700 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15701 final Acceleration by2 = new Acceleration(0.0,
15702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15703 calibrator.getInitialBiasYAsAcceleration(by2);
15704 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15705 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15706 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15707 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15708 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15709 final Acceleration bz2 = new Acceleration(0.0,
15710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15711 calibrator.getInitialBiasZAsAcceleration(bz2);
15712 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15713 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15714 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15715 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15716 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15717 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15718 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15719 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15720 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15721 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15722 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15723 final double[] bias1 = calibrator.getInitialBias();
15724 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15725 final double[] bias2 = new double[3];
15726 calibrator.getInitialBias(bias2);
15727 assertArrayEquals(bias1, bias2, 0.0);
15728 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15729 assertEquals(b1, ba);
15730 final Matrix b2 = new Matrix(3, 1);
15731 calibrator.getInitialBiasAsMatrix(b2);
15732 assertEquals(b1, b2);
15733 final Matrix ma1 = new Matrix(3, 3);
15734 ma1.setSubmatrix(0, 0,
15735 2, 2,
15736 new double[]{sx, myx, mzx,
15737 mxy, sy, mzy,
15738 mxz, myz, sz});
15739 assertEquals(calibrator.getInitialMa(), ma1);
15740 final Matrix ma2 = new Matrix(3, 3);
15741 calibrator.getInitialMa(ma2);
15742 assertEquals(ma1, ma2);
15743 assertSame(calibrator.getEcefPosition(), ecefPosition);
15744 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15745 final NEDPosition nedPosition1 = new NEDPosition();
15746 assertTrue(calibrator.getNedPosition(nedPosition1));
15747 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15748 assertSame(calibrator.getMeasurements(), measurements);
15749 assertFalse(calibrator.isCommonAxisUsed());
15750 assertSame(calibrator.getListener(), this);
15751 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15752 assertFalse(calibrator.isReady());
15753 assertFalse(calibrator.isRunning());
15754 assertNull(calibrator.getEstimatedBiases());
15755 assertFalse(calibrator.getEstimatedBiases(null));
15756 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15757 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15758 assertNull(calibrator.getEstimatedBiasFx());
15759 assertNull(calibrator.getEstimatedBiasFy());
15760 assertNull(calibrator.getEstimatedBiasFz());
15761 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15762 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15763 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15764 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15765 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15766 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15767 assertNull(calibrator.getEstimatedMa());
15768 assertNull(calibrator.getEstimatedSx());
15769 assertNull(calibrator.getEstimatedSy());
15770 assertNull(calibrator.getEstimatedSz());
15771 assertNull(calibrator.getEstimatedMxy());
15772 assertNull(calibrator.getEstimatedMxz());
15773 assertNull(calibrator.getEstimatedMyx());
15774 assertNull(calibrator.getEstimatedMyz());
15775 assertNull(calibrator.getEstimatedMzx());
15776 assertNull(calibrator.getEstimatedMzy());
15777 assertNull(calibrator.getEstimatedCovariance());
15778 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15779 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15780 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15781 assertNotNull(calibrator.getGroundTruthGravityNorm());
15782 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15783 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15784 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15785 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15786 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15787 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15788 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15789 }
15790
15791 @Test
15792 public void testConstructor129() throws WrongSizeException {
15793 final Matrix ba = generateBa();
15794 final double biasX = ba.getElementAtIndex(0);
15795 final double biasY = ba.getElementAtIndex(1);
15796 final double biasZ = ba.getElementAtIndex(2);
15797
15798 final Acceleration bx = new Acceleration(biasX,
15799 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15800 final Acceleration by = new Acceleration(biasY,
15801 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15802 final Acceleration bz = new Acceleration(biasZ,
15803 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15804
15805 final Matrix ma = generateMaCommonAxis();
15806 final double sx = ma.getElementAt(0, 0);
15807 final double sy = ma.getElementAt(1, 1);
15808 final double sz = ma.getElementAt(2, 2);
15809 final double mxy = ma.getElementAt(0, 1);
15810 final double mxz = ma.getElementAt(0, 2);
15811 final double myx = ma.getElementAt(1, 0);
15812 final double myz = ma.getElementAt(1, 2);
15813 final double mzx = ma.getElementAt(2, 0);
15814 final double mzy = ma.getElementAt(2, 1);
15815
15816 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15817 final double latitude = Math.toRadians(
15818 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15819 final double longitude = Math.toRadians(
15820 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15821 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15822 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15823 final NEDVelocity nedVelocity = new NEDVelocity();
15824 final ECEFPosition ecefPosition = new ECEFPosition();
15825 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15826 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15827 ecefPosition, ecefVelocity);
15828
15829 final KnownPositionAccelerometerCalibrator calibrator =
15830 new KnownPositionAccelerometerCalibrator(ecefPosition,
15831 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15832 myx, myz, mzx, mzy);
15833
15834
15835 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15836 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15837 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15838 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15839 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15840 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15841 final Acceleration bx2 = new Acceleration(0.0,
15842 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15843 calibrator.getInitialBiasXAsAcceleration(bx2);
15844 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15845 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15846 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15847 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15848 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15849 final Acceleration by2 = new Acceleration(0.0,
15850 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15851 calibrator.getInitialBiasYAsAcceleration(by2);
15852 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15853 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15854 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15855 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15856 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15857 final Acceleration bz2 = new Acceleration(0.0,
15858 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15859 calibrator.getInitialBiasZAsAcceleration(bz2);
15860 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15861 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15862 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15863 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15864 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15865 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15866 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15867 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15868 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15869 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15870 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15871 final double[] bias1 = calibrator.getInitialBias();
15872 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15873 final double[] bias2 = new double[3];
15874 calibrator.getInitialBias(bias2);
15875 assertArrayEquals(bias1, bias2, 0.0);
15876 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15877 assertEquals(b1, ba);
15878 final Matrix b2 = new Matrix(3, 1);
15879 calibrator.getInitialBiasAsMatrix(b2);
15880 assertEquals(b1, b2);
15881 final Matrix ma1 = new Matrix(3, 3);
15882 ma1.setSubmatrix(0, 0,
15883 2, 2,
15884 new double[]{sx, myx, mzx,
15885 mxy, sy, mzy,
15886 mxz, myz, sz});
15887 assertEquals(calibrator.getInitialMa(), ma1);
15888 final Matrix ma2 = new Matrix(3, 3);
15889 calibrator.getInitialMa(ma2);
15890 assertEquals(ma1, ma2);
15891 assertSame(calibrator.getEcefPosition(), ecefPosition);
15892 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15893 final NEDPosition nedPosition1 = new NEDPosition();
15894 assertTrue(calibrator.getNedPosition(nedPosition1));
15895 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15896 assertNull(calibrator.getMeasurements());
15897 assertTrue(calibrator.isCommonAxisUsed());
15898 assertNull(calibrator.getListener());
15899 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15900 assertFalse(calibrator.isReady());
15901 assertFalse(calibrator.isRunning());
15902 assertNull(calibrator.getEstimatedBiases());
15903 assertFalse(calibrator.getEstimatedBiases(null));
15904 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15905 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15906 assertNull(calibrator.getEstimatedBiasFx());
15907 assertNull(calibrator.getEstimatedBiasFy());
15908 assertNull(calibrator.getEstimatedBiasFz());
15909 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15910 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15911 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15912 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15913 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15914 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15915 assertNull(calibrator.getEstimatedMa());
15916 assertNull(calibrator.getEstimatedSx());
15917 assertNull(calibrator.getEstimatedSy());
15918 assertNull(calibrator.getEstimatedSz());
15919 assertNull(calibrator.getEstimatedMxy());
15920 assertNull(calibrator.getEstimatedMxz());
15921 assertNull(calibrator.getEstimatedMyx());
15922 assertNull(calibrator.getEstimatedMyz());
15923 assertNull(calibrator.getEstimatedMzx());
15924 assertNull(calibrator.getEstimatedMzy());
15925 assertNull(calibrator.getEstimatedCovariance());
15926 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15927 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15928 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15929 assertNotNull(calibrator.getGroundTruthGravityNorm());
15930 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15931 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15932 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15933 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15934 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15935 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15936 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15937 }
15938
15939 @Test
15940 public void testConstructor130() throws WrongSizeException {
15941 final Matrix ba = generateBa();
15942 final double biasX = ba.getElementAtIndex(0);
15943 final double biasY = ba.getElementAtIndex(1);
15944 final double biasZ = ba.getElementAtIndex(2);
15945
15946 final Acceleration bx = new Acceleration(biasX,
15947 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15948 final Acceleration by = new Acceleration(biasY,
15949 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15950 final Acceleration bz = new Acceleration(biasZ,
15951 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15952
15953 final Matrix ma = generateMaCommonAxis();
15954 final double sx = ma.getElementAt(0, 0);
15955 final double sy = ma.getElementAt(1, 1);
15956 final double sz = ma.getElementAt(2, 2);
15957 final double mxy = ma.getElementAt(0, 1);
15958 final double mxz = ma.getElementAt(0, 2);
15959 final double myx = ma.getElementAt(1, 0);
15960 final double myz = ma.getElementAt(1, 2);
15961 final double mzx = ma.getElementAt(2, 0);
15962 final double mzy = ma.getElementAt(2, 1);
15963
15964 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15965 final double latitude = Math.toRadians(
15966 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15967 final double longitude = Math.toRadians(
15968 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15969 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15970 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15971 final NEDVelocity nedVelocity = new NEDVelocity();
15972 final ECEFPosition ecefPosition = new ECEFPosition();
15973 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15974 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15975 ecefPosition, ecefVelocity);
15976
15977 final KnownPositionAccelerometerCalibrator calibrator =
15978 new KnownPositionAccelerometerCalibrator(ecefPosition,
15979 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15980 myx, myz, mzx, mzy, this);
15981
15982
15983 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15984 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15985 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15986 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15987 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15988 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15989 final Acceleration bx2 = new Acceleration(0.0,
15990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15991 calibrator.getInitialBiasXAsAcceleration(bx2);
15992 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15993 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15994 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15995 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15996 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15997 final Acceleration by2 = new Acceleration(0.0,
15998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15999 calibrator.getInitialBiasYAsAcceleration(by2);
16000 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16001 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16002 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16003 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16004 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16005 final Acceleration bz2 = new Acceleration(0.0,
16006 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16007 calibrator.getInitialBiasZAsAcceleration(bz2);
16008 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16009 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16010 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16011 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16012 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16013 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16014 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16015 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16016 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16017 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16018 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16019 final double[] bias1 = calibrator.getInitialBias();
16020 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16021 final double[] bias2 = new double[3];
16022 calibrator.getInitialBias(bias2);
16023 assertArrayEquals(bias1, bias2, 0.0);
16024 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16025 assertEquals(b1, ba);
16026 final Matrix b2 = new Matrix(3, 1);
16027 calibrator.getInitialBiasAsMatrix(b2);
16028 assertEquals(b1, b2);
16029 final Matrix ma1 = new Matrix(3, 3);
16030 ma1.setSubmatrix(0, 0,
16031 2, 2,
16032 new double[]{sx, myx, mzx,
16033 mxy, sy, mzy,
16034 mxz, myz, sz});
16035 assertEquals(calibrator.getInitialMa(), ma1);
16036 final Matrix ma2 = new Matrix(3, 3);
16037 calibrator.getInitialMa(ma2);
16038 assertEquals(ma1, ma2);
16039 assertSame(calibrator.getEcefPosition(), ecefPosition);
16040 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16041 final NEDPosition nedPosition1 = new NEDPosition();
16042 assertTrue(calibrator.getNedPosition(nedPosition1));
16043 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16044 assertNull(calibrator.getMeasurements());
16045 assertTrue(calibrator.isCommonAxisUsed());
16046 assertSame(calibrator.getListener(), this);
16047 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16048 assertFalse(calibrator.isReady());
16049 assertFalse(calibrator.isRunning());
16050 assertNull(calibrator.getEstimatedBiases());
16051 assertFalse(calibrator.getEstimatedBiases(null));
16052 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16053 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16054 assertNull(calibrator.getEstimatedBiasFx());
16055 assertNull(calibrator.getEstimatedBiasFy());
16056 assertNull(calibrator.getEstimatedBiasFz());
16057 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16058 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16059 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16060 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16061 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16062 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16063 assertNull(calibrator.getEstimatedMa());
16064 assertNull(calibrator.getEstimatedSx());
16065 assertNull(calibrator.getEstimatedSy());
16066 assertNull(calibrator.getEstimatedSz());
16067 assertNull(calibrator.getEstimatedMxy());
16068 assertNull(calibrator.getEstimatedMxz());
16069 assertNull(calibrator.getEstimatedMyx());
16070 assertNull(calibrator.getEstimatedMyz());
16071 assertNull(calibrator.getEstimatedMzx());
16072 assertNull(calibrator.getEstimatedMzy());
16073 assertNull(calibrator.getEstimatedCovariance());
16074 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16075 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16076 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16077 assertNotNull(calibrator.getGroundTruthGravityNorm());
16078 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16079 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16080 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16081 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16082 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16083 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16084 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16085 }
16086
16087 @Test
16088 public void testConstructor131() throws WrongSizeException {
16089 final Collection<StandardDeviationBodyKinematics> measurements =
16090 Collections.emptyList();
16091
16092 final Matrix ba = generateBa();
16093 final double biasX = ba.getElementAtIndex(0);
16094 final double biasY = ba.getElementAtIndex(1);
16095 final double biasZ = ba.getElementAtIndex(2);
16096
16097 final Acceleration bx = new Acceleration(biasX,
16098 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16099 final Acceleration by = new Acceleration(biasY,
16100 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16101 final Acceleration bz = new Acceleration(biasZ,
16102 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16103
16104 final Matrix ma = generateMaCommonAxis();
16105 final double sx = ma.getElementAt(0, 0);
16106 final double sy = ma.getElementAt(1, 1);
16107 final double sz = ma.getElementAt(2, 2);
16108 final double mxy = ma.getElementAt(0, 1);
16109 final double mxz = ma.getElementAt(0, 2);
16110 final double myx = ma.getElementAt(1, 0);
16111 final double myz = ma.getElementAt(1, 2);
16112 final double mzx = ma.getElementAt(2, 0);
16113 final double mzy = ma.getElementAt(2, 1);
16114
16115 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16116 final double latitude = Math.toRadians(
16117 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16118 final double longitude = Math.toRadians(
16119 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16120 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16121 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16122 final NEDVelocity nedVelocity = new NEDVelocity();
16123 final ECEFPosition ecefPosition = new ECEFPosition();
16124 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16125 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16126 ecefPosition, ecefVelocity);
16127
16128 final KnownPositionAccelerometerCalibrator calibrator =
16129 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16130 true, bx, by, bz, sx, sy, sz, mxy, mxz,
16131 myx, myz, mzx, mzy);
16132
16133
16134 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16135 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16136 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16137 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16138 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16139 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16140 final Acceleration bx2 = new Acceleration(0.0,
16141 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16142 calibrator.getInitialBiasXAsAcceleration(bx2);
16143 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16144 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16145 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16146 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16147 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16148 final Acceleration by2 = new Acceleration(0.0,
16149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16150 calibrator.getInitialBiasYAsAcceleration(by2);
16151 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16152 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16153 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16154 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16155 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16156 final Acceleration bz2 = new Acceleration(0.0,
16157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16158 calibrator.getInitialBiasZAsAcceleration(bz2);
16159 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16160 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16161 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16162 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16163 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16164 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16165 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16166 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16167 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16168 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16169 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16170 final double[] bias1 = calibrator.getInitialBias();
16171 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16172 final double[] bias2 = new double[3];
16173 calibrator.getInitialBias(bias2);
16174 assertArrayEquals(bias1, bias2, 0.0);
16175 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16176 assertEquals(b1, ba);
16177 final Matrix b2 = new Matrix(3, 1);
16178 calibrator.getInitialBiasAsMatrix(b2);
16179 assertEquals(b1, b2);
16180 final Matrix ma1 = new Matrix(3, 3);
16181 ma1.setSubmatrix(0, 0,
16182 2, 2,
16183 new double[]{sx, myx, mzx,
16184 mxy, sy, mzy,
16185 mxz, myz, sz});
16186 assertEquals(calibrator.getInitialMa(), ma1);
16187 final Matrix ma2 = new Matrix(3, 3);
16188 calibrator.getInitialMa(ma2);
16189 assertEquals(ma1, ma2);
16190 assertSame(calibrator.getEcefPosition(), ecefPosition);
16191 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16192 final NEDPosition nedPosition1 = new NEDPosition();
16193 assertTrue(calibrator.getNedPosition(nedPosition1));
16194 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16195 assertSame(calibrator.getMeasurements(), measurements);
16196 assertTrue(calibrator.isCommonAxisUsed());
16197 assertNull(calibrator.getListener());
16198 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16199 assertFalse(calibrator.isReady());
16200 assertFalse(calibrator.isRunning());
16201 assertNull(calibrator.getEstimatedBiases());
16202 assertFalse(calibrator.getEstimatedBiases(null));
16203 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16204 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16205 assertNull(calibrator.getEstimatedBiasFx());
16206 assertNull(calibrator.getEstimatedBiasFy());
16207 assertNull(calibrator.getEstimatedBiasFz());
16208 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16209 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16210 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16211 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16212 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16213 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16214 assertNull(calibrator.getEstimatedMa());
16215 assertNull(calibrator.getEstimatedSx());
16216 assertNull(calibrator.getEstimatedSy());
16217 assertNull(calibrator.getEstimatedSz());
16218 assertNull(calibrator.getEstimatedMxy());
16219 assertNull(calibrator.getEstimatedMxz());
16220 assertNull(calibrator.getEstimatedMyx());
16221 assertNull(calibrator.getEstimatedMyz());
16222 assertNull(calibrator.getEstimatedMzx());
16223 assertNull(calibrator.getEstimatedMzy());
16224 assertNull(calibrator.getEstimatedCovariance());
16225 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16226 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16227 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16228 assertNotNull(calibrator.getGroundTruthGravityNorm());
16229 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16230 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16231 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16232 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16233 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16234 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16235 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16236 }
16237
16238 @Test
16239 public void testConstructor132() throws WrongSizeException {
16240 final Collection<StandardDeviationBodyKinematics> measurements =
16241 Collections.emptyList();
16242
16243 final Matrix ba = generateBa();
16244 final double biasX = ba.getElementAtIndex(0);
16245 final double biasY = ba.getElementAtIndex(1);
16246 final double biasZ = ba.getElementAtIndex(2);
16247
16248 final Acceleration bx = new Acceleration(biasX,
16249 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16250 final Acceleration by = new Acceleration(biasY,
16251 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16252 final Acceleration bz = new Acceleration(biasZ,
16253 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16254
16255 final Matrix ma = generateMaCommonAxis();
16256 final double sx = ma.getElementAt(0, 0);
16257 final double sy = ma.getElementAt(1, 1);
16258 final double sz = ma.getElementAt(2, 2);
16259 final double mxy = ma.getElementAt(0, 1);
16260 final double mxz = ma.getElementAt(0, 2);
16261 final double myx = ma.getElementAt(1, 0);
16262 final double myz = ma.getElementAt(1, 2);
16263 final double mzx = ma.getElementAt(2, 0);
16264 final double mzy = ma.getElementAt(2, 1);
16265
16266 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16267 final double latitude = Math.toRadians(
16268 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16269 final double longitude = Math.toRadians(
16270 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16271 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16272 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16273 final NEDVelocity nedVelocity = new NEDVelocity();
16274 final ECEFPosition ecefPosition = new ECEFPosition();
16275 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16276 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16277 ecefPosition, ecefVelocity);
16278
16279 final KnownPositionAccelerometerCalibrator calibrator =
16280 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16281 true, bx, by, bz, sx, sy, sz, mxy, mxz,
16282 myx, myz, mzx, mzy, this);
16283
16284
16285 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16286 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16287 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16288 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16289 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16290 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16291 final Acceleration bx2 = new Acceleration(0.0,
16292 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16293 calibrator.getInitialBiasXAsAcceleration(bx2);
16294 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16295 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16296 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16297 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16298 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16299 final Acceleration by2 = new Acceleration(0.0,
16300 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16301 calibrator.getInitialBiasYAsAcceleration(by2);
16302 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16303 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16304 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16305 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16306 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16307 final Acceleration bz2 = new Acceleration(0.0,
16308 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16309 calibrator.getInitialBiasZAsAcceleration(bz2);
16310 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16311 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16312 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16313 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16314 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16315 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16316 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16317 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16318 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16319 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16320 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16321 final double[] bias1 = calibrator.getInitialBias();
16322 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16323 final double[] bias2 = new double[3];
16324 calibrator.getInitialBias(bias2);
16325 assertArrayEquals(bias1, bias2, 0.0);
16326 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16327 assertEquals(b1, ba);
16328 final Matrix b2 = new Matrix(3, 1);
16329 calibrator.getInitialBiasAsMatrix(b2);
16330 assertEquals(b1, b2);
16331 final Matrix ma1 = new Matrix(3, 3);
16332 ma1.setSubmatrix(0, 0,
16333 2, 2,
16334 new double[]{sx, myx, mzx,
16335 mxy, sy, mzy,
16336 mxz, myz, sz});
16337 assertEquals(calibrator.getInitialMa(), ma1);
16338 final Matrix ma2 = new Matrix(3, 3);
16339 calibrator.getInitialMa(ma2);
16340 assertEquals(ma1, ma2);
16341 assertSame(calibrator.getEcefPosition(), ecefPosition);
16342 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16343 final NEDPosition nedPosition1 = new NEDPosition();
16344 assertTrue(calibrator.getNedPosition(nedPosition1));
16345 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16346 assertSame(calibrator.getMeasurements(), measurements);
16347 assertTrue(calibrator.isCommonAxisUsed());
16348 assertSame(calibrator.getListener(), this);
16349 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16350 assertFalse(calibrator.isReady());
16351 assertFalse(calibrator.isRunning());
16352 assertNull(calibrator.getEstimatedBiases());
16353 assertFalse(calibrator.getEstimatedBiases(null));
16354 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16355 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16356 assertNull(calibrator.getEstimatedBiasFx());
16357 assertNull(calibrator.getEstimatedBiasFy());
16358 assertNull(calibrator.getEstimatedBiasFz());
16359 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16360 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16361 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16362 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16363 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16364 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16365 assertNull(calibrator.getEstimatedMa());
16366 assertNull(calibrator.getEstimatedSx());
16367 assertNull(calibrator.getEstimatedSy());
16368 assertNull(calibrator.getEstimatedSz());
16369 assertNull(calibrator.getEstimatedMxy());
16370 assertNull(calibrator.getEstimatedMxz());
16371 assertNull(calibrator.getEstimatedMyx());
16372 assertNull(calibrator.getEstimatedMyz());
16373 assertNull(calibrator.getEstimatedMzx());
16374 assertNull(calibrator.getEstimatedMzy());
16375 assertNull(calibrator.getEstimatedCovariance());
16376 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16377 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16378 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16379 assertNotNull(calibrator.getGroundTruthGravityNorm());
16380 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16381 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16382 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16383 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16384 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16385 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16386 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16387 }
16388
16389 @Test
16390 public void testConstructor133() throws WrongSizeException {
16391 final Matrix ba = generateBa();
16392 final double[] bias = ba.getBuffer();
16393 final double biasX = ba.getElementAtIndex(0);
16394 final double biasY = ba.getElementAtIndex(1);
16395 final double biasZ = ba.getElementAtIndex(2);
16396
16397 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16398 final double latitude = Math.toRadians(
16399 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16400 final double longitude = Math.toRadians(
16401 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16402 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16403 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16404 final NEDVelocity nedVelocity = new NEDVelocity();
16405 final ECEFPosition ecefPosition = new ECEFPosition();
16406 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16407 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16408 ecefPosition, ecefVelocity);
16409
16410 KnownPositionAccelerometerCalibrator calibrator =
16411 new KnownPositionAccelerometerCalibrator(ecefPosition, bias);
16412
16413
16414 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16415 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16416 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16417 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16418 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16419 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16420 final Acceleration bx2 = new Acceleration(0.0,
16421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16422 calibrator.getInitialBiasXAsAcceleration(bx2);
16423 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16424 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16425 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16426 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16427 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16428 final Acceleration by2 = new Acceleration(0.0,
16429 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16430 calibrator.getInitialBiasYAsAcceleration(by2);
16431 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16432 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16433 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16434 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16435 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16436 final Acceleration bz2 = new Acceleration(0.0,
16437 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16438 calibrator.getInitialBiasZAsAcceleration(bz2);
16439 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16440 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16441 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16442 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16443 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16444 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16445 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16446 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16447 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16448 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16449 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16450 final double[] bias1 = calibrator.getInitialBias();
16451 assertArrayEquals(bias1, bias, 0.0);
16452 final double[] bias2 = new double[3];
16453 calibrator.getInitialBias(bias2);
16454 assertArrayEquals(bias1, bias2, 0.0);
16455 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16456 assertEquals(b1, ba);
16457 final Matrix b2 = new Matrix(3, 1);
16458 calibrator.getInitialBiasAsMatrix(b2);
16459 assertEquals(b1, b2);
16460 final Matrix ma1 = calibrator.getInitialMa();
16461 assertEquals(ma1, new Matrix(3, 3));
16462 final Matrix ma2 = new Matrix(3, 3);
16463 calibrator.getInitialMa(ma2);
16464 assertEquals(ma1, ma2);
16465 assertSame(calibrator.getEcefPosition(), ecefPosition);
16466 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16467 final NEDPosition nedPosition1 = new NEDPosition();
16468 assertTrue(calibrator.getNedPosition(nedPosition1));
16469 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16470 assertNull(calibrator.getMeasurements());
16471 assertFalse(calibrator.isCommonAxisUsed());
16472 assertNull(calibrator.getListener());
16473 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16474 assertFalse(calibrator.isReady());
16475 assertFalse(calibrator.isRunning());
16476 assertNull(calibrator.getEstimatedBiases());
16477 assertFalse(calibrator.getEstimatedBiases(null));
16478 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16479 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16480 assertNull(calibrator.getEstimatedBiasFx());
16481 assertNull(calibrator.getEstimatedBiasFy());
16482 assertNull(calibrator.getEstimatedBiasFz());
16483 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16484 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16485 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16486 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16487 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16488 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16489 assertNull(calibrator.getEstimatedMa());
16490 assertNull(calibrator.getEstimatedSx());
16491 assertNull(calibrator.getEstimatedSy());
16492 assertNull(calibrator.getEstimatedSz());
16493 assertNull(calibrator.getEstimatedMxy());
16494 assertNull(calibrator.getEstimatedMxz());
16495 assertNull(calibrator.getEstimatedMyx());
16496 assertNull(calibrator.getEstimatedMyz());
16497 assertNull(calibrator.getEstimatedMzx());
16498 assertNull(calibrator.getEstimatedMzy());
16499 assertNull(calibrator.getEstimatedCovariance());
16500 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16501 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16502 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16503 assertNotNull(calibrator.getGroundTruthGravityNorm());
16504 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16505 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16506 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16507 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16508 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16509 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16510 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16511
16512
16513 calibrator = null;
16514 try {
16515 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16516 new double[1]);
16517 fail("IllegalArgumentException expected but not thrown");
16518 } catch (final IllegalArgumentException ignore) {
16519 }
16520 assertNull(calibrator);
16521 }
16522
16523 @Test
16524 public void testConstructor134() throws WrongSizeException {
16525 final Matrix ba = generateBa();
16526 final double[] bias = ba.getBuffer();
16527 final double biasX = ba.getElementAtIndex(0);
16528 final double biasY = ba.getElementAtIndex(1);
16529 final double biasZ = ba.getElementAtIndex(2);
16530
16531 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16532 final double latitude = Math.toRadians(
16533 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16534 final double longitude = Math.toRadians(
16535 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16536 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16537 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16538 final NEDVelocity nedVelocity = new NEDVelocity();
16539 final ECEFPosition ecefPosition = new ECEFPosition();
16540 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16541 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16542 ecefPosition, ecefVelocity);
16543
16544 KnownPositionAccelerometerCalibrator calibrator =
16545 new KnownPositionAccelerometerCalibrator(ecefPosition, bias,
16546 this);
16547
16548
16549 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16550 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16551 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16552 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16553 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16554 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16555 final Acceleration bx2 = new Acceleration(0.0,
16556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16557 calibrator.getInitialBiasXAsAcceleration(bx2);
16558 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16559 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16560 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16561 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16562 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16563 final Acceleration by2 = new Acceleration(0.0,
16564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16565 calibrator.getInitialBiasYAsAcceleration(by2);
16566 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16567 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16568 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16569 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16570 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16571 final Acceleration bz2 = new Acceleration(0.0,
16572 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16573 calibrator.getInitialBiasZAsAcceleration(bz2);
16574 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16575 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16576 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16577 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16578 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16579 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16580 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16581 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16582 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16583 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16584 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16585 final double[] bias1 = calibrator.getInitialBias();
16586 assertArrayEquals(bias1, bias, 0.0);
16587 final double[] bias2 = new double[3];
16588 calibrator.getInitialBias(bias2);
16589 assertArrayEquals(bias1, bias2, 0.0);
16590 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16591 assertEquals(b1, ba);
16592 final Matrix b2 = new Matrix(3, 1);
16593 calibrator.getInitialBiasAsMatrix(b2);
16594 assertEquals(b1, b2);
16595 final Matrix ma1 = calibrator.getInitialMa();
16596 assertEquals(ma1, new Matrix(3, 3));
16597 final Matrix ma2 = new Matrix(3, 3);
16598 calibrator.getInitialMa(ma2);
16599 assertEquals(ma1, ma2);
16600 assertSame(calibrator.getEcefPosition(), ecefPosition);
16601 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16602 final NEDPosition nedPosition1 = new NEDPosition();
16603 assertTrue(calibrator.getNedPosition(nedPosition1));
16604 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16605 assertNull(calibrator.getMeasurements());
16606 assertFalse(calibrator.isCommonAxisUsed());
16607 assertSame(calibrator.getListener(), this);
16608 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16609 assertFalse(calibrator.isReady());
16610 assertFalse(calibrator.isRunning());
16611 assertNull(calibrator.getEstimatedBiases());
16612 assertFalse(calibrator.getEstimatedBiases(null));
16613 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16614 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16615 assertNull(calibrator.getEstimatedBiasFx());
16616 assertNull(calibrator.getEstimatedBiasFy());
16617 assertNull(calibrator.getEstimatedBiasFz());
16618 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16619 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16620 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16621 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16622 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16623 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16624 assertNull(calibrator.getEstimatedMa());
16625 assertNull(calibrator.getEstimatedSx());
16626 assertNull(calibrator.getEstimatedSy());
16627 assertNull(calibrator.getEstimatedSz());
16628 assertNull(calibrator.getEstimatedMxy());
16629 assertNull(calibrator.getEstimatedMxz());
16630 assertNull(calibrator.getEstimatedMyx());
16631 assertNull(calibrator.getEstimatedMyz());
16632 assertNull(calibrator.getEstimatedMzx());
16633 assertNull(calibrator.getEstimatedMzy());
16634 assertNull(calibrator.getEstimatedCovariance());
16635 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16636 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16637 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16638 assertNotNull(calibrator.getGroundTruthGravityNorm());
16639 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16640 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16641 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16642 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16643 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16644 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16645 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16646
16647
16648 calibrator = null;
16649 try {
16650 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16651 new double[1], this);
16652 fail("IllegalArgumentException expected but not thrown");
16653 } catch (final IllegalArgumentException ignore) {
16654 }
16655 assertNull(calibrator);
16656 }
16657
16658 @Test
16659 public void testConstructor135() throws WrongSizeException {
16660 final Collection<StandardDeviationBodyKinematics> measurements =
16661 Collections.emptyList();
16662
16663 final Matrix ba = generateBa();
16664 final double[] bias = ba.getBuffer();
16665 final double biasX = ba.getElementAtIndex(0);
16666 final double biasY = ba.getElementAtIndex(1);
16667 final double biasZ = ba.getElementAtIndex(2);
16668
16669 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16670 final double latitude = Math.toRadians(
16671 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16672 final double longitude = Math.toRadians(
16673 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16674 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16675 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16676 final NEDVelocity nedVelocity = new NEDVelocity();
16677 final ECEFPosition ecefPosition = new ECEFPosition();
16678 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16679 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16680 ecefPosition, ecefVelocity);
16681
16682 KnownPositionAccelerometerCalibrator calibrator =
16683 new KnownPositionAccelerometerCalibrator(ecefPosition,
16684 measurements, bias);
16685
16686
16687 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16688 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16689 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16690 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16691 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16692 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16693 final Acceleration bx2 = new Acceleration(0.0,
16694 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16695 calibrator.getInitialBiasXAsAcceleration(bx2);
16696 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16697 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16698 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16699 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16700 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16701 final Acceleration by2 = new Acceleration(0.0,
16702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16703 calibrator.getInitialBiasYAsAcceleration(by2);
16704 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16705 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16706 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16707 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16708 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16709 final Acceleration bz2 = new Acceleration(0.0,
16710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16711 calibrator.getInitialBiasZAsAcceleration(bz2);
16712 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16713 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16714 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16715 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16716 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16717 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16718 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16719 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16720 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16721 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16722 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16723 final double[] bias1 = calibrator.getInitialBias();
16724 assertArrayEquals(bias1, bias, 0.0);
16725 final double[] bias2 = new double[3];
16726 calibrator.getInitialBias(bias2);
16727 assertArrayEquals(bias1, bias2, 0.0);
16728 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16729 assertEquals(b1, ba);
16730 final Matrix b2 = new Matrix(3, 1);
16731 calibrator.getInitialBiasAsMatrix(b2);
16732 assertEquals(b1, b2);
16733 final Matrix ma1 = calibrator.getInitialMa();
16734 assertEquals(ma1, new Matrix(3, 3));
16735 final Matrix ma2 = new Matrix(3, 3);
16736 calibrator.getInitialMa(ma2);
16737 assertEquals(ma1, ma2);
16738 assertSame(calibrator.getEcefPosition(), ecefPosition);
16739 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16740 final NEDPosition nedPosition1 = new NEDPosition();
16741 assertTrue(calibrator.getNedPosition(nedPosition1));
16742 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16743 assertSame(calibrator.getMeasurements(), measurements);
16744 assertFalse(calibrator.isCommonAxisUsed());
16745 assertNull(calibrator.getListener());
16746 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16747 assertFalse(calibrator.isReady());
16748 assertFalse(calibrator.isRunning());
16749 assertNull(calibrator.getEstimatedBiases());
16750 assertFalse(calibrator.getEstimatedBiases(null));
16751 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16752 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16753 assertNull(calibrator.getEstimatedBiasFx());
16754 assertNull(calibrator.getEstimatedBiasFy());
16755 assertNull(calibrator.getEstimatedBiasFz());
16756 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16757 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16758 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16759 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16760 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16761 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16762 assertNull(calibrator.getEstimatedMa());
16763 assertNull(calibrator.getEstimatedSx());
16764 assertNull(calibrator.getEstimatedSy());
16765 assertNull(calibrator.getEstimatedSz());
16766 assertNull(calibrator.getEstimatedMxy());
16767 assertNull(calibrator.getEstimatedMxz());
16768 assertNull(calibrator.getEstimatedMyx());
16769 assertNull(calibrator.getEstimatedMyz());
16770 assertNull(calibrator.getEstimatedMzx());
16771 assertNull(calibrator.getEstimatedMzy());
16772 assertNull(calibrator.getEstimatedCovariance());
16773 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16774 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16775 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16776 assertNotNull(calibrator.getGroundTruthGravityNorm());
16777 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16778 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16779 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16780 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16781 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16782 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16783 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16784
16785
16786 calibrator = null;
16787 try {
16788 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16789 measurements, new double[1]);
16790 fail("IllegalArgumentException expected but not thrown");
16791 } catch (final IllegalArgumentException ignore) {
16792 }
16793 assertNull(calibrator);
16794 }
16795
16796 @Test
16797 public void testConstructor136() throws WrongSizeException {
16798 final Collection<StandardDeviationBodyKinematics> measurements =
16799 Collections.emptyList();
16800
16801 final Matrix ba = generateBa();
16802 final double[] bias = ba.getBuffer();
16803 final double biasX = ba.getElementAtIndex(0);
16804 final double biasY = ba.getElementAtIndex(1);
16805 final double biasZ = ba.getElementAtIndex(2);
16806
16807 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16808 final double latitude = Math.toRadians(
16809 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16810 final double longitude = Math.toRadians(
16811 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16812 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16813 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16814 final NEDVelocity nedVelocity = new NEDVelocity();
16815 final ECEFPosition ecefPosition = new ECEFPosition();
16816 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16817 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16818 ecefPosition, ecefVelocity);
16819
16820 KnownPositionAccelerometerCalibrator calibrator =
16821 new KnownPositionAccelerometerCalibrator(ecefPosition,
16822 measurements, bias, this);
16823
16824
16825 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16826 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16827 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16828 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16829 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16830 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16831 final Acceleration bx2 = new Acceleration(0.0,
16832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16833 calibrator.getInitialBiasXAsAcceleration(bx2);
16834 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16835 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16836 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16837 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16838 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16839 final Acceleration by2 = new Acceleration(0.0,
16840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16841 calibrator.getInitialBiasYAsAcceleration(by2);
16842 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16843 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16844 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16845 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16846 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16847 final Acceleration bz2 = new Acceleration(0.0,
16848 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16849 calibrator.getInitialBiasZAsAcceleration(bz2);
16850 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16851 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16852 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16853 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16854 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16855 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16856 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16857 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16858 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16859 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16860 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16861 final double[] bias1 = calibrator.getInitialBias();
16862 assertArrayEquals(bias1, bias, 0.0);
16863 final double[] bias2 = new double[3];
16864 calibrator.getInitialBias(bias2);
16865 assertArrayEquals(bias1, bias2, 0.0);
16866 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16867 assertEquals(b1, ba);
16868 final Matrix b2 = new Matrix(3, 1);
16869 calibrator.getInitialBiasAsMatrix(b2);
16870 assertEquals(b1, b2);
16871 final Matrix ma1 = calibrator.getInitialMa();
16872 assertEquals(ma1, new Matrix(3, 3));
16873 final Matrix ma2 = new Matrix(3, 3);
16874 calibrator.getInitialMa(ma2);
16875 assertEquals(ma1, ma2);
16876 assertSame(calibrator.getEcefPosition(), ecefPosition);
16877 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16878 final NEDPosition nedPosition1 = new NEDPosition();
16879 assertTrue(calibrator.getNedPosition(nedPosition1));
16880 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16881 assertSame(calibrator.getMeasurements(), measurements);
16882 assertFalse(calibrator.isCommonAxisUsed());
16883 assertSame(calibrator.getListener(), this);
16884 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16885 assertFalse(calibrator.isReady());
16886 assertFalse(calibrator.isRunning());
16887 assertNull(calibrator.getEstimatedBiases());
16888 assertFalse(calibrator.getEstimatedBiases(null));
16889 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16890 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16891 assertNull(calibrator.getEstimatedBiasFx());
16892 assertNull(calibrator.getEstimatedBiasFy());
16893 assertNull(calibrator.getEstimatedBiasFz());
16894 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16895 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16896 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16897 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16898 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16899 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16900 assertNull(calibrator.getEstimatedMa());
16901 assertNull(calibrator.getEstimatedSx());
16902 assertNull(calibrator.getEstimatedSy());
16903 assertNull(calibrator.getEstimatedSz());
16904 assertNull(calibrator.getEstimatedMxy());
16905 assertNull(calibrator.getEstimatedMxz());
16906 assertNull(calibrator.getEstimatedMyx());
16907 assertNull(calibrator.getEstimatedMyz());
16908 assertNull(calibrator.getEstimatedMzx());
16909 assertNull(calibrator.getEstimatedMzy());
16910 assertNull(calibrator.getEstimatedCovariance());
16911 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16912 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16913 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16914 assertNotNull(calibrator.getGroundTruthGravityNorm());
16915 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16916 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16917 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16918 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16919 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16920 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16921 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16922
16923
16924 calibrator = null;
16925 try {
16926 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16927 measurements, new double[1], this);
16928 fail("IllegalArgumentException expected but not thrown");
16929 } catch (final IllegalArgumentException ignore) {
16930 }
16931 assertNull(calibrator);
16932 }
16933
16934 @Test
16935 public void testConstructor137() throws WrongSizeException {
16936 final Matrix ba = generateBa();
16937 final double[] bias = ba.getBuffer();
16938 final double biasX = ba.getElementAtIndex(0);
16939 final double biasY = ba.getElementAtIndex(1);
16940 final double biasZ = ba.getElementAtIndex(2);
16941
16942 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16943 final double latitude = Math.toRadians(
16944 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16945 final double longitude = Math.toRadians(
16946 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16947 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16948 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16949 final NEDVelocity nedVelocity = new NEDVelocity();
16950 final ECEFPosition ecefPosition = new ECEFPosition();
16951 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16952 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16953 ecefPosition, ecefVelocity);
16954
16955 KnownPositionAccelerometerCalibrator calibrator =
16956 new KnownPositionAccelerometerCalibrator(ecefPosition,
16957 true, bias);
16958
16959
16960 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16961 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16962 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16963 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16964 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16965 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16966 final Acceleration bx2 = new Acceleration(0.0,
16967 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16968 calibrator.getInitialBiasXAsAcceleration(bx2);
16969 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16970 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16971 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16972 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16973 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16974 final Acceleration by2 = new Acceleration(0.0,
16975 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16976 calibrator.getInitialBiasYAsAcceleration(by2);
16977 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16978 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16979 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16980 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16981 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16982 final Acceleration bz2 = new Acceleration(0.0,
16983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16984 calibrator.getInitialBiasZAsAcceleration(bz2);
16985 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16986 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16987 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16988 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16989 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16990 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16991 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16992 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16993 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16994 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16995 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16996 final double[] bias1 = calibrator.getInitialBias();
16997 assertArrayEquals(bias1, bias, 0.0);
16998 final double[] bias2 = new double[3];
16999 calibrator.getInitialBias(bias2);
17000 assertArrayEquals(bias1, bias2, 0.0);
17001 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17002 assertEquals(b1, ba);
17003 final Matrix b2 = new Matrix(3, 1);
17004 calibrator.getInitialBiasAsMatrix(b2);
17005 assertEquals(b1, b2);
17006 final Matrix ma1 = calibrator.getInitialMa();
17007 assertEquals(ma1, new Matrix(3, 3));
17008 final Matrix ma2 = new Matrix(3, 3);
17009 calibrator.getInitialMa(ma2);
17010 assertEquals(ma1, ma2);
17011 assertSame(calibrator.getEcefPosition(), ecefPosition);
17012 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17013 final NEDPosition nedPosition1 = new NEDPosition();
17014 assertTrue(calibrator.getNedPosition(nedPosition1));
17015 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17016 assertNull(calibrator.getMeasurements());
17017 assertTrue(calibrator.isCommonAxisUsed());
17018 assertNull(calibrator.getListener());
17019 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17020 assertFalse(calibrator.isReady());
17021 assertFalse(calibrator.isRunning());
17022 assertNull(calibrator.getEstimatedBiases());
17023 assertFalse(calibrator.getEstimatedBiases(null));
17024 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17025 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17026 assertNull(calibrator.getEstimatedBiasFx());
17027 assertNull(calibrator.getEstimatedBiasFy());
17028 assertNull(calibrator.getEstimatedBiasFz());
17029 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17030 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17031 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17032 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17033 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17034 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17035 assertNull(calibrator.getEstimatedMa());
17036 assertNull(calibrator.getEstimatedSx());
17037 assertNull(calibrator.getEstimatedSy());
17038 assertNull(calibrator.getEstimatedSz());
17039 assertNull(calibrator.getEstimatedMxy());
17040 assertNull(calibrator.getEstimatedMxz());
17041 assertNull(calibrator.getEstimatedMyx());
17042 assertNull(calibrator.getEstimatedMyz());
17043 assertNull(calibrator.getEstimatedMzx());
17044 assertNull(calibrator.getEstimatedMzy());
17045 assertNull(calibrator.getEstimatedCovariance());
17046 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17047 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17048 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17049 assertNotNull(calibrator.getGroundTruthGravityNorm());
17050 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17051 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17052 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17053 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17054 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17055 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17056 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17057
17058
17059 calibrator = null;
17060 try {
17061 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17062 true, new double[1]);
17063 fail("IllegalArgumentException expected but not thrown");
17064 } catch (final IllegalArgumentException ignore) {
17065 }
17066 assertNull(calibrator);
17067 }
17068
17069 @Test
17070 public void testConstructor138() throws WrongSizeException {
17071 final Matrix ba = generateBa();
17072 final double[] bias = ba.getBuffer();
17073 final double biasX = ba.getElementAtIndex(0);
17074 final double biasY = ba.getElementAtIndex(1);
17075 final double biasZ = ba.getElementAtIndex(2);
17076
17077 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17078 final double latitude = Math.toRadians(
17079 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17080 final double longitude = Math.toRadians(
17081 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17082 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17083 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17084 final NEDVelocity nedVelocity = new NEDVelocity();
17085 final ECEFPosition ecefPosition = new ECEFPosition();
17086 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17087 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17088 ecefPosition, ecefVelocity);
17089
17090 KnownPositionAccelerometerCalibrator calibrator =
17091 new KnownPositionAccelerometerCalibrator(ecefPosition,
17092 true, bias, this);
17093
17094
17095 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17096 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17097 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17098 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17099 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17100 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17101 final Acceleration bx2 = new Acceleration(0.0,
17102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17103 calibrator.getInitialBiasXAsAcceleration(bx2);
17104 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17105 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17106 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17107 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17108 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17109 final Acceleration by2 = new Acceleration(0.0,
17110 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17111 calibrator.getInitialBiasYAsAcceleration(by2);
17112 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17113 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17114 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17115 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17116 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17117 final Acceleration bz2 = new Acceleration(0.0,
17118 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17119 calibrator.getInitialBiasZAsAcceleration(bz2);
17120 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17121 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17122 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17123 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17124 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17125 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17126 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17127 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17128 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17129 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17130 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17131 final double[] bias1 = calibrator.getInitialBias();
17132 assertArrayEquals(bias1, bias, 0.0);
17133 final double[] bias2 = new double[3];
17134 calibrator.getInitialBias(bias2);
17135 assertArrayEquals(bias1, bias2, 0.0);
17136 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17137 assertEquals(b1, ba);
17138 final Matrix b2 = new Matrix(3, 1);
17139 calibrator.getInitialBiasAsMatrix(b2);
17140 assertEquals(b1, b2);
17141 final Matrix ma1 = calibrator.getInitialMa();
17142 assertEquals(ma1, new Matrix(3, 3));
17143 final Matrix ma2 = new Matrix(3, 3);
17144 calibrator.getInitialMa(ma2);
17145 assertEquals(ma1, ma2);
17146 assertSame(calibrator.getEcefPosition(), ecefPosition);
17147 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17148 final NEDPosition nedPosition1 = new NEDPosition();
17149 assertTrue(calibrator.getNedPosition(nedPosition1));
17150 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17151 assertNull(calibrator.getMeasurements());
17152 assertTrue(calibrator.isCommonAxisUsed());
17153 assertSame(calibrator.getListener(), this);
17154 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17155 assertFalse(calibrator.isReady());
17156 assertFalse(calibrator.isRunning());
17157 assertNull(calibrator.getEstimatedBiases());
17158 assertFalse(calibrator.getEstimatedBiases(null));
17159 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17160 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17161 assertNull(calibrator.getEstimatedBiasFx());
17162 assertNull(calibrator.getEstimatedBiasFy());
17163 assertNull(calibrator.getEstimatedBiasFz());
17164 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17165 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17166 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17167 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17168 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17169 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17170 assertNull(calibrator.getEstimatedMa());
17171 assertNull(calibrator.getEstimatedSx());
17172 assertNull(calibrator.getEstimatedSy());
17173 assertNull(calibrator.getEstimatedSz());
17174 assertNull(calibrator.getEstimatedMxy());
17175 assertNull(calibrator.getEstimatedMxz());
17176 assertNull(calibrator.getEstimatedMyx());
17177 assertNull(calibrator.getEstimatedMyz());
17178 assertNull(calibrator.getEstimatedMzx());
17179 assertNull(calibrator.getEstimatedMzy());
17180 assertNull(calibrator.getEstimatedCovariance());
17181 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17182 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17183 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17184 assertNotNull(calibrator.getGroundTruthGravityNorm());
17185 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17186 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17187 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17188 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17189 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17190 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17191 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17192
17193
17194 calibrator = null;
17195 try {
17196 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17197 true, new double[1], this);
17198 fail("IllegalArgumentException expected but not thrown");
17199 } catch (final IllegalArgumentException ignore) {
17200 }
17201 assertNull(calibrator);
17202 }
17203
17204 @Test
17205 public void testConstructor139() throws WrongSizeException {
17206 final Collection<StandardDeviationBodyKinematics> measurements =
17207 Collections.emptyList();
17208
17209 final Matrix ba = generateBa();
17210 final double[] bias = ba.getBuffer();
17211 final double biasX = ba.getElementAtIndex(0);
17212 final double biasY = ba.getElementAtIndex(1);
17213 final double biasZ = ba.getElementAtIndex(2);
17214
17215 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17216 final double latitude = Math.toRadians(
17217 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17218 final double longitude = Math.toRadians(
17219 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17220 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17221 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17222 final NEDVelocity nedVelocity = new NEDVelocity();
17223 final ECEFPosition ecefPosition = new ECEFPosition();
17224 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17225 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17226 ecefPosition, ecefVelocity);
17227
17228 KnownPositionAccelerometerCalibrator calibrator =
17229 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17230 true, bias);
17231
17232
17233 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17234 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17235 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17236 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17237 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17238 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17239 final Acceleration bx2 = new Acceleration(0.0,
17240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17241 calibrator.getInitialBiasXAsAcceleration(bx2);
17242 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17243 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17244 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17245 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17246 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17247 final Acceleration by2 = new Acceleration(0.0,
17248 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17249 calibrator.getInitialBiasYAsAcceleration(by2);
17250 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17251 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17252 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17253 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17254 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17255 final Acceleration bz2 = new Acceleration(0.0,
17256 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17257 calibrator.getInitialBiasZAsAcceleration(bz2);
17258 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17259 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17260 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17261 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17262 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17263 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17264 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17265 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17266 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17267 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17268 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17269 final double[] bias1 = calibrator.getInitialBias();
17270 assertArrayEquals(bias1, bias, 0.0);
17271 final double[] bias2 = new double[3];
17272 calibrator.getInitialBias(bias2);
17273 assertArrayEquals(bias1, bias2, 0.0);
17274 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17275 assertEquals(b1, ba);
17276 final Matrix b2 = new Matrix(3, 1);
17277 calibrator.getInitialBiasAsMatrix(b2);
17278 assertEquals(b1, b2);
17279 final Matrix ma1 = calibrator.getInitialMa();
17280 assertEquals(ma1, new Matrix(3, 3));
17281 final Matrix ma2 = new Matrix(3, 3);
17282 calibrator.getInitialMa(ma2);
17283 assertEquals(ma1, ma2);
17284 assertSame(calibrator.getEcefPosition(), ecefPosition);
17285 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17286 final NEDPosition nedPosition1 = new NEDPosition();
17287 assertTrue(calibrator.getNedPosition(nedPosition1));
17288 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17289 assertSame(calibrator.getMeasurements(), measurements);
17290 assertTrue(calibrator.isCommonAxisUsed());
17291 assertNull(calibrator.getListener());
17292 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17293 assertFalse(calibrator.isReady());
17294 assertFalse(calibrator.isRunning());
17295 assertNull(calibrator.getEstimatedBiases());
17296 assertFalse(calibrator.getEstimatedBiases(null));
17297 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17298 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17299 assertNull(calibrator.getEstimatedBiasFx());
17300 assertNull(calibrator.getEstimatedBiasFy());
17301 assertNull(calibrator.getEstimatedBiasFz());
17302 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17303 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17304 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17305 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17306 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17307 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17308 assertNull(calibrator.getEstimatedMa());
17309 assertNull(calibrator.getEstimatedSx());
17310 assertNull(calibrator.getEstimatedSy());
17311 assertNull(calibrator.getEstimatedSz());
17312 assertNull(calibrator.getEstimatedMxy());
17313 assertNull(calibrator.getEstimatedMxz());
17314 assertNull(calibrator.getEstimatedMyx());
17315 assertNull(calibrator.getEstimatedMyz());
17316 assertNull(calibrator.getEstimatedMzx());
17317 assertNull(calibrator.getEstimatedMzy());
17318 assertNull(calibrator.getEstimatedCovariance());
17319 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17320 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17321 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17322 assertNotNull(calibrator.getGroundTruthGravityNorm());
17323 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17324 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17325 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17326 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17327 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17328 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17329 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17330
17331
17332 calibrator = null;
17333 try {
17334 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17335 measurements, true, new double[1]);
17336 fail("IllegalArgumentException expected but not thrown");
17337 } catch (final IllegalArgumentException ignore) {
17338 }
17339 assertNull(calibrator);
17340 }
17341
17342 @Test
17343 public void testConstructor140() throws WrongSizeException {
17344 final Collection<StandardDeviationBodyKinematics> measurements =
17345 Collections.emptyList();
17346
17347 final Matrix ba = generateBa();
17348 final double[] bias = ba.getBuffer();
17349 final double biasX = ba.getElementAtIndex(0);
17350 final double biasY = ba.getElementAtIndex(1);
17351 final double biasZ = ba.getElementAtIndex(2);
17352
17353 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17354 final double latitude = Math.toRadians(
17355 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17356 final double longitude = Math.toRadians(
17357 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17358 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17359 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17360 final NEDVelocity nedVelocity = new NEDVelocity();
17361 final ECEFPosition ecefPosition = new ECEFPosition();
17362 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17363 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17364 ecefPosition, ecefVelocity);
17365
17366 KnownPositionAccelerometerCalibrator calibrator =
17367 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17368 true, bias, this);
17369
17370
17371 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17372 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17373 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17374 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17375 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17376 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17377 final Acceleration bx2 = new Acceleration(0.0,
17378 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17379 calibrator.getInitialBiasXAsAcceleration(bx2);
17380 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17381 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17382 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17383 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17384 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17385 final Acceleration by2 = new Acceleration(0.0,
17386 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17387 calibrator.getInitialBiasYAsAcceleration(by2);
17388 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17389 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17390 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17391 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17392 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17393 final Acceleration bz2 = new Acceleration(0.0,
17394 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17395 calibrator.getInitialBiasZAsAcceleration(bz2);
17396 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17397 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17398 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17399 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17400 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17401 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17402 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17403 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17404 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17405 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17406 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17407 final double[] bias1 = calibrator.getInitialBias();
17408 assertArrayEquals(bias1, bias, 0.0);
17409 final double[] bias2 = new double[3];
17410 calibrator.getInitialBias(bias2);
17411 assertArrayEquals(bias1, bias2, 0.0);
17412 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17413 assertEquals(b1, ba);
17414 final Matrix b2 = new Matrix(3, 1);
17415 calibrator.getInitialBiasAsMatrix(b2);
17416 assertEquals(b1, b2);
17417 final Matrix ma1 = calibrator.getInitialMa();
17418 assertEquals(ma1, new Matrix(3, 3));
17419 final Matrix ma2 = new Matrix(3, 3);
17420 calibrator.getInitialMa(ma2);
17421 assertEquals(ma1, ma2);
17422 assertSame(calibrator.getEcefPosition(), ecefPosition);
17423 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17424 final NEDPosition nedPosition1 = new NEDPosition();
17425 assertTrue(calibrator.getNedPosition(nedPosition1));
17426 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17427 assertSame(calibrator.getMeasurements(), measurements);
17428 assertTrue(calibrator.isCommonAxisUsed());
17429 assertSame(calibrator.getListener(), this);
17430 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17431 assertFalse(calibrator.isReady());
17432 assertFalse(calibrator.isRunning());
17433 assertNull(calibrator.getEstimatedBiases());
17434 assertFalse(calibrator.getEstimatedBiases(null));
17435 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17436 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17437 assertNull(calibrator.getEstimatedBiasFx());
17438 assertNull(calibrator.getEstimatedBiasFy());
17439 assertNull(calibrator.getEstimatedBiasFz());
17440 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17441 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17442 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17443 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17444 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17445 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17446 assertNull(calibrator.getEstimatedMa());
17447 assertNull(calibrator.getEstimatedSx());
17448 assertNull(calibrator.getEstimatedSy());
17449 assertNull(calibrator.getEstimatedSz());
17450 assertNull(calibrator.getEstimatedMxy());
17451 assertNull(calibrator.getEstimatedMxz());
17452 assertNull(calibrator.getEstimatedMyx());
17453 assertNull(calibrator.getEstimatedMyz());
17454 assertNull(calibrator.getEstimatedMzx());
17455 assertNull(calibrator.getEstimatedMzy());
17456 assertNull(calibrator.getEstimatedCovariance());
17457 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17458 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17459 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17460 assertNotNull(calibrator.getGroundTruthGravityNorm());
17461 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17462 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17463 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17464 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17465 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17466 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17467 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17468
17469
17470 calibrator = null;
17471 try {
17472 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17473 measurements, true, new double[1], this);
17474 fail("IllegalArgumentException expected but not thrown");
17475 } catch (final IllegalArgumentException ignore) {
17476 }
17477 assertNull(calibrator);
17478 }
17479
17480 @Test
17481 public void testConstructor141() throws WrongSizeException {
17482 final Matrix ba = generateBa();
17483 final double[] bias = ba.getBuffer();
17484 final double biasX = ba.getElementAtIndex(0);
17485 final double biasY = ba.getElementAtIndex(1);
17486 final double biasZ = ba.getElementAtIndex(2);
17487
17488 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17489 final double latitude = Math.toRadians(
17490 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17491 final double longitude = Math.toRadians(
17492 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17493 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17494 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17495 final NEDVelocity nedVelocity = new NEDVelocity();
17496 final ECEFPosition ecefPosition = new ECEFPosition();
17497 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17498 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17499 ecefPosition, ecefVelocity);
17500
17501 KnownPositionAccelerometerCalibrator calibrator =
17502 new KnownPositionAccelerometerCalibrator(ecefPosition, ba);
17503
17504
17505 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17506 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17507 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17508 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17509 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17510 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17511 final Acceleration bx2 = new Acceleration(0.0,
17512 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17513 calibrator.getInitialBiasXAsAcceleration(bx2);
17514 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17515 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17516 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17517 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17518 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17519 final Acceleration by2 = new Acceleration(0.0,
17520 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17521 calibrator.getInitialBiasYAsAcceleration(by2);
17522 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17523 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17524 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17525 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17526 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17527 final Acceleration bz2 = new Acceleration(0.0,
17528 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17529 calibrator.getInitialBiasZAsAcceleration(bz2);
17530 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17531 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17532 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17533 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17534 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17535 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17536 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17537 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17538 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17539 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17540 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17541 final double[] bias1 = calibrator.getInitialBias();
17542 assertArrayEquals(bias1, bias, 0.0);
17543 final double[] bias2 = new double[3];
17544 calibrator.getInitialBias(bias2);
17545 assertArrayEquals(bias1, bias2, 0.0);
17546 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17547 assertEquals(b1, ba);
17548 final Matrix b2 = new Matrix(3, 1);
17549 calibrator.getInitialBiasAsMatrix(b2);
17550 assertEquals(b1, b2);
17551 final Matrix ma1 = calibrator.getInitialMa();
17552 assertEquals(ma1, new Matrix(3, 3));
17553 final Matrix ma2 = new Matrix(3, 3);
17554 calibrator.getInitialMa(ma2);
17555 assertEquals(ma1, ma2);
17556 assertSame(calibrator.getEcefPosition(), ecefPosition);
17557 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17558 final NEDPosition nedPosition1 = new NEDPosition();
17559 assertTrue(calibrator.getNedPosition(nedPosition1));
17560 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17561 assertNull(calibrator.getMeasurements());
17562 assertFalse(calibrator.isCommonAxisUsed());
17563 assertNull(calibrator.getListener());
17564 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17565 assertFalse(calibrator.isReady());
17566 assertFalse(calibrator.isRunning());
17567 assertNull(calibrator.getEstimatedBiases());
17568 assertFalse(calibrator.getEstimatedBiases(null));
17569 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17570 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17571 assertNull(calibrator.getEstimatedBiasFx());
17572 assertNull(calibrator.getEstimatedBiasFy());
17573 assertNull(calibrator.getEstimatedBiasFz());
17574 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17575 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17576 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17577 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17578 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17579 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17580 assertNull(calibrator.getEstimatedMa());
17581 assertNull(calibrator.getEstimatedSx());
17582 assertNull(calibrator.getEstimatedSy());
17583 assertNull(calibrator.getEstimatedSz());
17584 assertNull(calibrator.getEstimatedMxy());
17585 assertNull(calibrator.getEstimatedMxz());
17586 assertNull(calibrator.getEstimatedMyx());
17587 assertNull(calibrator.getEstimatedMyz());
17588 assertNull(calibrator.getEstimatedMzx());
17589 assertNull(calibrator.getEstimatedMzy());
17590 assertNull(calibrator.getEstimatedCovariance());
17591 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17592 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17593 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17594 assertNotNull(calibrator.getGroundTruthGravityNorm());
17595 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17596 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17597 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17598 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17599 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17600 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17601 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17602
17603
17604 calibrator = null;
17605 try {
17606 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17607 new Matrix(1, 1));
17608 fail("IllegalArgumentException expected but not thrown");
17609 } catch (final IllegalArgumentException ignore) {
17610 }
17611 try {
17612 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17613 new Matrix(1, 3));
17614 fail("IllegalArgumentException expected but not thrown");
17615 } catch (final IllegalArgumentException ignore) {
17616 }
17617 assertNull(calibrator);
17618 }
17619
17620 @Test
17621 public void testConstructor142() throws WrongSizeException {
17622 final Matrix ba = generateBa();
17623 final double[] bias = ba.getBuffer();
17624 final double biasX = ba.getElementAtIndex(0);
17625 final double biasY = ba.getElementAtIndex(1);
17626 final double biasZ = ba.getElementAtIndex(2);
17627
17628 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17629 final double latitude = Math.toRadians(
17630 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17631 final double longitude = Math.toRadians(
17632 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17633 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17634 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17635 final NEDVelocity nedVelocity = new NEDVelocity();
17636 final ECEFPosition ecefPosition = new ECEFPosition();
17637 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17638 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17639 ecefPosition, ecefVelocity);
17640
17641 KnownPositionAccelerometerCalibrator calibrator =
17642 new KnownPositionAccelerometerCalibrator(ecefPosition, ba,
17643 this);
17644
17645
17646 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17647 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17648 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17649 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17650 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17651 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17652 final Acceleration bx2 = new Acceleration(0.0,
17653 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17654 calibrator.getInitialBiasXAsAcceleration(bx2);
17655 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17656 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17657 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17658 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17659 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17660 final Acceleration by2 = new Acceleration(0.0,
17661 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17662 calibrator.getInitialBiasYAsAcceleration(by2);
17663 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17664 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17665 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17666 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17667 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17668 final Acceleration bz2 = new Acceleration(0.0,
17669 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17670 calibrator.getInitialBiasZAsAcceleration(bz2);
17671 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17672 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17673 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17674 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17675 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17676 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17677 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17678 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17679 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17680 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17681 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17682 final double[] bias1 = calibrator.getInitialBias();
17683 assertArrayEquals(bias1, bias, 0.0);
17684 final double[] bias2 = new double[3];
17685 calibrator.getInitialBias(bias2);
17686 assertArrayEquals(bias1, bias2, 0.0);
17687 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17688 assertEquals(b1, ba);
17689 final Matrix b2 = new Matrix(3, 1);
17690 calibrator.getInitialBiasAsMatrix(b2);
17691 assertEquals(b1, b2);
17692 final Matrix ma1 = calibrator.getInitialMa();
17693 assertEquals(ma1, new Matrix(3, 3));
17694 final Matrix ma2 = new Matrix(3, 3);
17695 calibrator.getInitialMa(ma2);
17696 assertEquals(ma1, ma2);
17697 assertSame(calibrator.getEcefPosition(), ecefPosition);
17698 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17699 final NEDPosition nedPosition1 = new NEDPosition();
17700 assertTrue(calibrator.getNedPosition(nedPosition1));
17701 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17702 assertNull(calibrator.getMeasurements());
17703 assertFalse(calibrator.isCommonAxisUsed());
17704 assertSame(calibrator.getListener(), this);
17705 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17706 assertFalse(calibrator.isReady());
17707 assertFalse(calibrator.isRunning());
17708 assertNull(calibrator.getEstimatedBiases());
17709 assertFalse(calibrator.getEstimatedBiases(null));
17710 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17711 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17712 assertNull(calibrator.getEstimatedBiasFx());
17713 assertNull(calibrator.getEstimatedBiasFy());
17714 assertNull(calibrator.getEstimatedBiasFz());
17715 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17716 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17717 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17718 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17719 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17720 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17721 assertNull(calibrator.getEstimatedMa());
17722 assertNull(calibrator.getEstimatedSx());
17723 assertNull(calibrator.getEstimatedSy());
17724 assertNull(calibrator.getEstimatedSz());
17725 assertNull(calibrator.getEstimatedMxy());
17726 assertNull(calibrator.getEstimatedMxz());
17727 assertNull(calibrator.getEstimatedMyx());
17728 assertNull(calibrator.getEstimatedMyz());
17729 assertNull(calibrator.getEstimatedMzx());
17730 assertNull(calibrator.getEstimatedMzy());
17731 assertNull(calibrator.getEstimatedCovariance());
17732 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17733 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17734 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17735 assertNotNull(calibrator.getGroundTruthGravityNorm());
17736 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17737 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17738 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17739 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17740 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17741 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17742 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17743
17744
17745 calibrator = null;
17746 try {
17747 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17748 new Matrix(1, 1), this);
17749 fail("IllegalArgumentException expected but not thrown");
17750 } catch (final IllegalArgumentException ignore) {
17751 }
17752 try {
17753 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17754 new Matrix(1, 3), this);
17755 fail("IllegalArgumentException expected but not thrown");
17756 } catch (final IllegalArgumentException ignore) {
17757 }
17758 assertNull(calibrator);
17759 }
17760
17761 @Test
17762 public void testConstructor143() throws WrongSizeException {
17763 final Collection<StandardDeviationBodyKinematics> measurements =
17764 Collections.emptyList();
17765
17766 final Matrix ba = generateBa();
17767 final double[] bias = ba.getBuffer();
17768 final double biasX = ba.getElementAtIndex(0);
17769 final double biasY = ba.getElementAtIndex(1);
17770 final double biasZ = ba.getElementAtIndex(2);
17771
17772 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17773 final double latitude = Math.toRadians(
17774 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17775 final double longitude = Math.toRadians(
17776 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17777 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17778 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17779 final NEDVelocity nedVelocity = new NEDVelocity();
17780 final ECEFPosition ecefPosition = new ECEFPosition();
17781 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17782 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17783 ecefPosition, ecefVelocity);
17784
17785 KnownPositionAccelerometerCalibrator calibrator =
17786 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17787 ba);
17788
17789
17790 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17791 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17792 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17793 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17794 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17795 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17796 final Acceleration bx2 = new Acceleration(0.0,
17797 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17798 calibrator.getInitialBiasXAsAcceleration(bx2);
17799 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17800 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17801 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17802 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17803 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17804 final Acceleration by2 = new Acceleration(0.0,
17805 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17806 calibrator.getInitialBiasYAsAcceleration(by2);
17807 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17808 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17809 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17810 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17811 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17812 final Acceleration bz2 = new Acceleration(0.0,
17813 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17814 calibrator.getInitialBiasZAsAcceleration(bz2);
17815 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17816 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17817 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17818 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17819 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17820 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17821 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17822 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17823 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17824 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17825 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17826 final double[] bias1 = calibrator.getInitialBias();
17827 assertArrayEquals(bias1, bias, 0.0);
17828 final double[] bias2 = new double[3];
17829 calibrator.getInitialBias(bias2);
17830 assertArrayEquals(bias1, bias2, 0.0);
17831 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17832 assertEquals(b1, ba);
17833 final Matrix b2 = new Matrix(3, 1);
17834 calibrator.getInitialBiasAsMatrix(b2);
17835 assertEquals(b1, b2);
17836 final Matrix ma1 = calibrator.getInitialMa();
17837 assertEquals(ma1, new Matrix(3, 3));
17838 final Matrix ma2 = new Matrix(3, 3);
17839 calibrator.getInitialMa(ma2);
17840 assertEquals(ma1, ma2);
17841 assertSame(calibrator.getEcefPosition(), ecefPosition);
17842 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17843 final NEDPosition nedPosition1 = new NEDPosition();
17844 assertTrue(calibrator.getNedPosition(nedPosition1));
17845 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17846 assertSame(calibrator.getMeasurements(), measurements);
17847 assertFalse(calibrator.isCommonAxisUsed());
17848 assertNull(calibrator.getListener());
17849 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17850 assertFalse(calibrator.isReady());
17851 assertFalse(calibrator.isRunning());
17852 assertNull(calibrator.getEstimatedBiases());
17853 assertFalse(calibrator.getEstimatedBiases(null));
17854 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17855 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17856 assertNull(calibrator.getEstimatedBiasFx());
17857 assertNull(calibrator.getEstimatedBiasFy());
17858 assertNull(calibrator.getEstimatedBiasFz());
17859 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17860 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17861 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17862 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17863 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17864 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17865 assertNull(calibrator.getEstimatedMa());
17866 assertNull(calibrator.getEstimatedSx());
17867 assertNull(calibrator.getEstimatedSy());
17868 assertNull(calibrator.getEstimatedSz());
17869 assertNull(calibrator.getEstimatedMxy());
17870 assertNull(calibrator.getEstimatedMxz());
17871 assertNull(calibrator.getEstimatedMyx());
17872 assertNull(calibrator.getEstimatedMyz());
17873 assertNull(calibrator.getEstimatedMzx());
17874 assertNull(calibrator.getEstimatedMzy());
17875 assertNull(calibrator.getEstimatedCovariance());
17876 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17877 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17878 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17879 assertNotNull(calibrator.getGroundTruthGravityNorm());
17880 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17881 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17882 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17883 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17884 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17885 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17886 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17887
17888
17889 calibrator = null;
17890 try {
17891 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17892 measurements, new Matrix(1, 1));
17893 fail("IllegalArgumentException expected but not thrown");
17894 } catch (final IllegalArgumentException ignore) {
17895 }
17896 try {
17897 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17898 measurements, new Matrix(1, 3));
17899 fail("IllegalArgumentException expected but not thrown");
17900 } catch (final IllegalArgumentException ignore) {
17901 }
17902 assertNull(calibrator);
17903 }
17904
17905 @Test
17906 public void testConstructor144() throws WrongSizeException {
17907 final Collection<StandardDeviationBodyKinematics> measurements =
17908 Collections.emptyList();
17909
17910 final Matrix ba = generateBa();
17911 final double[] bias = ba.getBuffer();
17912 final double biasX = ba.getElementAtIndex(0);
17913 final double biasY = ba.getElementAtIndex(1);
17914 final double biasZ = ba.getElementAtIndex(2);
17915
17916 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17917 final double latitude = Math.toRadians(
17918 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17919 final double longitude = Math.toRadians(
17920 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17921 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17922 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17923 final NEDVelocity nedVelocity = new NEDVelocity();
17924 final ECEFPosition ecefPosition = new ECEFPosition();
17925 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17926 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17927 ecefPosition, ecefVelocity);
17928
17929 KnownPositionAccelerometerCalibrator calibrator =
17930 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17931 ba, this);
17932
17933
17934 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17935 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17936 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17937 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17938 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17939 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17940 final Acceleration bx2 = new Acceleration(0.0,
17941 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17942 calibrator.getInitialBiasXAsAcceleration(bx2);
17943 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17944 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17945 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17946 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17947 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17948 final Acceleration by2 = new Acceleration(0.0,
17949 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17950 calibrator.getInitialBiasYAsAcceleration(by2);
17951 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17952 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17953 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17954 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17955 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17956 final Acceleration bz2 = new Acceleration(0.0,
17957 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17958 calibrator.getInitialBiasZAsAcceleration(bz2);
17959 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17960 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17961 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17962 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17963 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17964 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17965 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17966 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17967 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17968 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17969 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17970 final double[] bias1 = calibrator.getInitialBias();
17971 assertArrayEquals(bias1, bias, 0.0);
17972 final double[] bias2 = new double[3];
17973 calibrator.getInitialBias(bias2);
17974 assertArrayEquals(bias1, bias2, 0.0);
17975 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17976 assertEquals(b1, ba);
17977 final Matrix b2 = new Matrix(3, 1);
17978 calibrator.getInitialBiasAsMatrix(b2);
17979 assertEquals(b1, b2);
17980 final Matrix ma1 = calibrator.getInitialMa();
17981 assertEquals(ma1, new Matrix(3, 3));
17982 final Matrix ma2 = new Matrix(3, 3);
17983 calibrator.getInitialMa(ma2);
17984 assertEquals(ma1, ma2);
17985 assertSame(calibrator.getEcefPosition(), ecefPosition);
17986 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17987 final NEDPosition nedPosition1 = new NEDPosition();
17988 assertTrue(calibrator.getNedPosition(nedPosition1));
17989 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17990 assertSame(calibrator.getMeasurements(), measurements);
17991 assertFalse(calibrator.isCommonAxisUsed());
17992 assertSame(calibrator.getListener(), this);
17993 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17994 assertFalse(calibrator.isReady());
17995 assertFalse(calibrator.isRunning());
17996 assertNull(calibrator.getEstimatedBiases());
17997 assertFalse(calibrator.getEstimatedBiases(null));
17998 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17999 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18000 assertNull(calibrator.getEstimatedBiasFx());
18001 assertNull(calibrator.getEstimatedBiasFy());
18002 assertNull(calibrator.getEstimatedBiasFz());
18003 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18004 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18005 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18006 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18007 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18008 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18009 assertNull(calibrator.getEstimatedMa());
18010 assertNull(calibrator.getEstimatedSx());
18011 assertNull(calibrator.getEstimatedSy());
18012 assertNull(calibrator.getEstimatedSz());
18013 assertNull(calibrator.getEstimatedMxy());
18014 assertNull(calibrator.getEstimatedMxz());
18015 assertNull(calibrator.getEstimatedMyx());
18016 assertNull(calibrator.getEstimatedMyz());
18017 assertNull(calibrator.getEstimatedMzx());
18018 assertNull(calibrator.getEstimatedMzy());
18019 assertNull(calibrator.getEstimatedCovariance());
18020 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18021 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18022 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18023 assertNotNull(calibrator.getGroundTruthGravityNorm());
18024 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18025 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18026 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18027 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18028 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18029 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18030 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18031
18032
18033 calibrator = null;
18034 try {
18035 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18036 measurements, new Matrix(1, 1), this);
18037 fail("IllegalArgumentException expected but not thrown");
18038 } catch (final IllegalArgumentException ignore) {
18039 }
18040 try {
18041 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18042 measurements, new Matrix(1, 3), this);
18043 fail("IllegalArgumentException expected but not thrown");
18044 } catch (final IllegalArgumentException ignore) {
18045 }
18046 assertNull(calibrator);
18047 }
18048
18049 @Test
18050 public void testConstructor145() throws WrongSizeException {
18051 final Matrix ba = generateBa();
18052 final double[] bias = ba.getBuffer();
18053 final double biasX = ba.getElementAtIndex(0);
18054 final double biasY = ba.getElementAtIndex(1);
18055 final double biasZ = ba.getElementAtIndex(2);
18056
18057 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18058 final double latitude = Math.toRadians(
18059 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18060 final double longitude = Math.toRadians(
18061 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18062 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18063 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18064 final NEDVelocity nedVelocity = new NEDVelocity();
18065 final ECEFPosition ecefPosition = new ECEFPosition();
18066 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18067 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18068 ecefPosition, ecefVelocity);
18069
18070 KnownPositionAccelerometerCalibrator calibrator =
18071 new KnownPositionAccelerometerCalibrator(ecefPosition,
18072 true, ba);
18073
18074
18075 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18076 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18077 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18078 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18079 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18080 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18081 final Acceleration bx2 = new Acceleration(0.0,
18082 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18083 calibrator.getInitialBiasXAsAcceleration(bx2);
18084 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18085 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18086 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18087 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18088 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18089 final Acceleration by2 = new Acceleration(0.0,
18090 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18091 calibrator.getInitialBiasYAsAcceleration(by2);
18092 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18093 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18094 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18095 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18096 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18097 final Acceleration bz2 = new Acceleration(0.0,
18098 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18099 calibrator.getInitialBiasZAsAcceleration(bz2);
18100 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18101 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18102 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18103 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18104 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18105 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18106 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18107 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18108 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18109 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18110 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18111 final double[] bias1 = calibrator.getInitialBias();
18112 assertArrayEquals(bias1, bias, 0.0);
18113 final double[] bias2 = new double[3];
18114 calibrator.getInitialBias(bias2);
18115 assertArrayEquals(bias1, bias2, 0.0);
18116 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18117 assertEquals(b1, ba);
18118 final Matrix b2 = new Matrix(3, 1);
18119 calibrator.getInitialBiasAsMatrix(b2);
18120 assertEquals(b1, b2);
18121 final Matrix ma1 = calibrator.getInitialMa();
18122 assertEquals(ma1, new Matrix(3, 3));
18123 final Matrix ma2 = new Matrix(3, 3);
18124 calibrator.getInitialMa(ma2);
18125 assertEquals(ma1, ma2);
18126 assertSame(calibrator.getEcefPosition(), ecefPosition);
18127 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18128 final NEDPosition nedPosition1 = new NEDPosition();
18129 assertTrue(calibrator.getNedPosition(nedPosition1));
18130 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18131 assertNull(calibrator.getMeasurements());
18132 assertTrue(calibrator.isCommonAxisUsed());
18133 assertNull(calibrator.getListener());
18134 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18135 assertFalse(calibrator.isReady());
18136 assertFalse(calibrator.isRunning());
18137 assertNull(calibrator.getEstimatedBiases());
18138 assertFalse(calibrator.getEstimatedBiases(null));
18139 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18140 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18141 assertNull(calibrator.getEstimatedBiasFx());
18142 assertNull(calibrator.getEstimatedBiasFy());
18143 assertNull(calibrator.getEstimatedBiasFz());
18144 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18145 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18146 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18147 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18148 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18149 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18150 assertNull(calibrator.getEstimatedMa());
18151 assertNull(calibrator.getEstimatedSx());
18152 assertNull(calibrator.getEstimatedSy());
18153 assertNull(calibrator.getEstimatedSz());
18154 assertNull(calibrator.getEstimatedMxy());
18155 assertNull(calibrator.getEstimatedMxz());
18156 assertNull(calibrator.getEstimatedMyx());
18157 assertNull(calibrator.getEstimatedMyz());
18158 assertNull(calibrator.getEstimatedMzx());
18159 assertNull(calibrator.getEstimatedMzy());
18160 assertNull(calibrator.getEstimatedCovariance());
18161 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18162 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18163 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18164 assertNotNull(calibrator.getGroundTruthGravityNorm());
18165 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18166 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18167 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18168 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18169 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18170 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18171 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18172
18173
18174 calibrator = null;
18175 try {
18176 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18177 true, new Matrix(1, 1));
18178 fail("IllegalArgumentException expected but not thrown");
18179 } catch (final IllegalArgumentException ignore) {
18180 }
18181 try {
18182 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18183 true, new Matrix(1, 3));
18184 fail("IllegalArgumentException expected but not thrown");
18185 } catch (final IllegalArgumentException ignore) {
18186 }
18187 assertNull(calibrator);
18188 }
18189
18190 @Test
18191 public void testConstructor146() throws WrongSizeException {
18192 final Matrix ba = generateBa();
18193 final double[] bias = ba.getBuffer();
18194 final double biasX = ba.getElementAtIndex(0);
18195 final double biasY = ba.getElementAtIndex(1);
18196 final double biasZ = ba.getElementAtIndex(2);
18197
18198 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18199 final double latitude = Math.toRadians(
18200 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18201 final double longitude = Math.toRadians(
18202 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18203 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18204 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18205 final NEDVelocity nedVelocity = new NEDVelocity();
18206 final ECEFPosition ecefPosition = new ECEFPosition();
18207 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18208 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18209 ecefPosition, ecefVelocity);
18210
18211 KnownPositionAccelerometerCalibrator calibrator =
18212 new KnownPositionAccelerometerCalibrator(ecefPosition,
18213 true, ba, this);
18214
18215
18216 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18217 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18218 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18219 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18220 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18221 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18222 final Acceleration bx2 = new Acceleration(0.0,
18223 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18224 calibrator.getInitialBiasXAsAcceleration(bx2);
18225 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18226 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18227 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18228 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18229 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18230 final Acceleration by2 = new Acceleration(0.0,
18231 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18232 calibrator.getInitialBiasYAsAcceleration(by2);
18233 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18234 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18235 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18236 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18237 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18238 final Acceleration bz2 = new Acceleration(0.0,
18239 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18240 calibrator.getInitialBiasZAsAcceleration(bz2);
18241 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18242 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18243 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18244 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18245 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18246 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18247 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18248 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18249 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18250 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18251 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18252 final double[] bias1 = calibrator.getInitialBias();
18253 assertArrayEquals(bias1, bias, 0.0);
18254 final double[] bias2 = new double[3];
18255 calibrator.getInitialBias(bias2);
18256 assertArrayEquals(bias1, bias2, 0.0);
18257 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18258 assertEquals(b1, ba);
18259 final Matrix b2 = new Matrix(3, 1);
18260 calibrator.getInitialBiasAsMatrix(b2);
18261 assertEquals(b1, b2);
18262 final Matrix ma1 = calibrator.getInitialMa();
18263 assertEquals(ma1, new Matrix(3, 3));
18264 final Matrix ma2 = new Matrix(3, 3);
18265 calibrator.getInitialMa(ma2);
18266 assertEquals(ma1, ma2);
18267 assertSame(calibrator.getEcefPosition(), ecefPosition);
18268 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18269 final NEDPosition nedPosition1 = new NEDPosition();
18270 assertTrue(calibrator.getNedPosition(nedPosition1));
18271 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18272 assertNull(calibrator.getMeasurements());
18273 assertTrue(calibrator.isCommonAxisUsed());
18274 assertSame(calibrator.getListener(), this);
18275 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18276 assertFalse(calibrator.isReady());
18277 assertFalse(calibrator.isRunning());
18278 assertNull(calibrator.getEstimatedBiases());
18279 assertFalse(calibrator.getEstimatedBiases(null));
18280 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18281 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18282 assertNull(calibrator.getEstimatedBiasFx());
18283 assertNull(calibrator.getEstimatedBiasFy());
18284 assertNull(calibrator.getEstimatedBiasFz());
18285 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18286 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18287 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18288 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18289 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18290 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18291 assertNull(calibrator.getEstimatedMa());
18292 assertNull(calibrator.getEstimatedSx());
18293 assertNull(calibrator.getEstimatedSy());
18294 assertNull(calibrator.getEstimatedSz());
18295 assertNull(calibrator.getEstimatedMxy());
18296 assertNull(calibrator.getEstimatedMxz());
18297 assertNull(calibrator.getEstimatedMyx());
18298 assertNull(calibrator.getEstimatedMyz());
18299 assertNull(calibrator.getEstimatedMzx());
18300 assertNull(calibrator.getEstimatedMzy());
18301 assertNull(calibrator.getEstimatedCovariance());
18302 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18303 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18304 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18305 assertNotNull(calibrator.getGroundTruthGravityNorm());
18306 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18307 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18308 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18309 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18310 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18311 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18312 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18313
18314
18315 calibrator = null;
18316 try {
18317 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18318 true, new Matrix(1, 1),
18319 this);
18320 fail("IllegalArgumentException expected but not thrown");
18321 } catch (final IllegalArgumentException ignore) {
18322 }
18323 try {
18324 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18325 true, new Matrix(1, 3),
18326 this);
18327 fail("IllegalArgumentException expected but not thrown");
18328 } catch (final IllegalArgumentException ignore) {
18329 }
18330 assertNull(calibrator);
18331 }
18332
18333 @Test
18334 public void testConstructor147() throws WrongSizeException {
18335 final Collection<StandardDeviationBodyKinematics> measurements =
18336 Collections.emptyList();
18337
18338 final Matrix ba = generateBa();
18339 final double[] bias = ba.getBuffer();
18340 final double biasX = ba.getElementAtIndex(0);
18341 final double biasY = ba.getElementAtIndex(1);
18342 final double biasZ = ba.getElementAtIndex(2);
18343
18344 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18345 final double latitude = Math.toRadians(
18346 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18347 final double longitude = Math.toRadians(
18348 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18349 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18350 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18351 final NEDVelocity nedVelocity = new NEDVelocity();
18352 final ECEFPosition ecefPosition = new ECEFPosition();
18353 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18354 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18355 ecefPosition, ecefVelocity);
18356
18357 KnownPositionAccelerometerCalibrator calibrator =
18358 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18359 true, ba);
18360
18361
18362 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18363 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18364 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18365 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18366 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18367 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18368 final Acceleration bx2 = new Acceleration(0.0,
18369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18370 calibrator.getInitialBiasXAsAcceleration(bx2);
18371 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18372 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18373 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18374 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18375 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18376 final Acceleration by2 = new Acceleration(0.0,
18377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18378 calibrator.getInitialBiasYAsAcceleration(by2);
18379 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18380 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18381 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18382 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18383 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18384 final Acceleration bz2 = new Acceleration(0.0,
18385 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18386 calibrator.getInitialBiasZAsAcceleration(bz2);
18387 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18388 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18389 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18390 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18391 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18392 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18393 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18394 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18395 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18396 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18397 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18398 final double[] bias1 = calibrator.getInitialBias();
18399 assertArrayEquals(bias1, bias, 0.0);
18400 final double[] bias2 = new double[3];
18401 calibrator.getInitialBias(bias2);
18402 assertArrayEquals(bias1, bias2, 0.0);
18403 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18404 assertEquals(b1, ba);
18405 final Matrix b2 = new Matrix(3, 1);
18406 calibrator.getInitialBiasAsMatrix(b2);
18407 assertEquals(b1, b2);
18408 final Matrix ma1 = calibrator.getInitialMa();
18409 assertEquals(ma1, new Matrix(3, 3));
18410 final Matrix ma2 = new Matrix(3, 3);
18411 calibrator.getInitialMa(ma2);
18412 assertEquals(ma1, ma2);
18413 assertSame(calibrator.getEcefPosition(), ecefPosition);
18414 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18415 final NEDPosition nedPosition1 = new NEDPosition();
18416 assertTrue(calibrator.getNedPosition(nedPosition1));
18417 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18418 assertSame(calibrator.getMeasurements(), measurements);
18419 assertTrue(calibrator.isCommonAxisUsed());
18420 assertNull(calibrator.getListener());
18421 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18422 assertFalse(calibrator.isReady());
18423 assertFalse(calibrator.isRunning());
18424 assertNull(calibrator.getEstimatedBiases());
18425 assertFalse(calibrator.getEstimatedBiases(null));
18426 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18427 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18428 assertNull(calibrator.getEstimatedBiasFx());
18429 assertNull(calibrator.getEstimatedBiasFy());
18430 assertNull(calibrator.getEstimatedBiasFz());
18431 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18432 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18433 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18434 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18435 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18436 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18437 assertNull(calibrator.getEstimatedMa());
18438 assertNull(calibrator.getEstimatedSx());
18439 assertNull(calibrator.getEstimatedSy());
18440 assertNull(calibrator.getEstimatedSz());
18441 assertNull(calibrator.getEstimatedMxy());
18442 assertNull(calibrator.getEstimatedMxz());
18443 assertNull(calibrator.getEstimatedMyx());
18444 assertNull(calibrator.getEstimatedMyz());
18445 assertNull(calibrator.getEstimatedMzx());
18446 assertNull(calibrator.getEstimatedMzy());
18447 assertNull(calibrator.getEstimatedCovariance());
18448 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18449 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18450 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18451 assertNotNull(calibrator.getGroundTruthGravityNorm());
18452 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18453 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18454 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18455 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18456 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18457 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18458 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18459
18460
18461 calibrator = null;
18462 try {
18463 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18464 measurements, true,
18465 new Matrix(1, 1));
18466 fail("IllegalArgumentException expected but not thrown");
18467 } catch (final IllegalArgumentException ignore) {
18468 }
18469 try {
18470 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18471 measurements, true,
18472 new Matrix(1, 3));
18473 fail("IllegalArgumentException expected but not thrown");
18474 } catch (final IllegalArgumentException ignore) {
18475 }
18476 assertNull(calibrator);
18477 }
18478
18479 @Test
18480 public void testConstructor148() throws WrongSizeException {
18481 final Collection<StandardDeviationBodyKinematics> measurements =
18482 Collections.emptyList();
18483
18484 final Matrix ba = generateBa();
18485 final double[] bias = ba.getBuffer();
18486 final double biasX = ba.getElementAtIndex(0);
18487 final double biasY = ba.getElementAtIndex(1);
18488 final double biasZ = ba.getElementAtIndex(2);
18489
18490 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18491 final double latitude = Math.toRadians(
18492 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18493 final double longitude = Math.toRadians(
18494 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18495 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18496 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18497 final NEDVelocity nedVelocity = new NEDVelocity();
18498 final ECEFPosition ecefPosition = new ECEFPosition();
18499 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18500 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18501 ecefPosition, ecefVelocity);
18502
18503 KnownPositionAccelerometerCalibrator calibrator =
18504 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18505 true, ba, this);
18506
18507
18508 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18509 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18510 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18511 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18512 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18513 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18514 final Acceleration bx2 = new Acceleration(0.0,
18515 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18516 calibrator.getInitialBiasXAsAcceleration(bx2);
18517 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18518 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18519 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18520 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18521 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18522 final Acceleration by2 = new Acceleration(0.0,
18523 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18524 calibrator.getInitialBiasYAsAcceleration(by2);
18525 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18526 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18527 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18528 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18529 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18530 final Acceleration bz2 = new Acceleration(0.0,
18531 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18532 calibrator.getInitialBiasZAsAcceleration(bz2);
18533 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18534 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18535 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18536 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18537 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18538 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18539 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18540 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18541 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18542 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18543 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18544 final double[] bias1 = calibrator.getInitialBias();
18545 assertArrayEquals(bias1, bias, 0.0);
18546 final double[] bias2 = new double[3];
18547 calibrator.getInitialBias(bias2);
18548 assertArrayEquals(bias1, bias2, 0.0);
18549 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18550 assertEquals(b1, ba);
18551 final Matrix b2 = new Matrix(3, 1);
18552 calibrator.getInitialBiasAsMatrix(b2);
18553 assertEquals(b1, b2);
18554 final Matrix ma1 = calibrator.getInitialMa();
18555 assertEquals(ma1, new Matrix(3, 3));
18556 final Matrix ma2 = new Matrix(3, 3);
18557 calibrator.getInitialMa(ma2);
18558 assertEquals(ma1, ma2);
18559 assertSame(calibrator.getEcefPosition(), ecefPosition);
18560 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18561 final NEDPosition nedPosition1 = new NEDPosition();
18562 assertTrue(calibrator.getNedPosition(nedPosition1));
18563 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18564 assertSame(calibrator.getMeasurements(), measurements);
18565 assertTrue(calibrator.isCommonAxisUsed());
18566 assertSame(calibrator.getListener(), this);
18567 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18568 assertFalse(calibrator.isReady());
18569 assertFalse(calibrator.isRunning());
18570 assertNull(calibrator.getEstimatedBiases());
18571 assertFalse(calibrator.getEstimatedBiases(null));
18572 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18573 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18574 assertNull(calibrator.getEstimatedBiasFx());
18575 assertNull(calibrator.getEstimatedBiasFy());
18576 assertNull(calibrator.getEstimatedBiasFz());
18577 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18578 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18579 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18580 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18581 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18582 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18583 assertNull(calibrator.getEstimatedMa());
18584 assertNull(calibrator.getEstimatedSx());
18585 assertNull(calibrator.getEstimatedSy());
18586 assertNull(calibrator.getEstimatedSz());
18587 assertNull(calibrator.getEstimatedMxy());
18588 assertNull(calibrator.getEstimatedMxz());
18589 assertNull(calibrator.getEstimatedMyx());
18590 assertNull(calibrator.getEstimatedMyz());
18591 assertNull(calibrator.getEstimatedMzx());
18592 assertNull(calibrator.getEstimatedMzy());
18593 assertNull(calibrator.getEstimatedCovariance());
18594 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18595 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18596 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18597 assertNotNull(calibrator.getGroundTruthGravityNorm());
18598 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18599 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18600 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18601 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18602 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18603 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18604 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18605
18606
18607 calibrator = null;
18608 try {
18609 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18610 measurements, true,
18611 new Matrix(1, 1), this);
18612 fail("IllegalArgumentException expected but not thrown");
18613 } catch (final IllegalArgumentException ignore) {
18614 }
18615 try {
18616 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18617 measurements, true,
18618 new Matrix(1, 3), this);
18619 fail("IllegalArgumentException expected but not thrown");
18620 } catch (final IllegalArgumentException ignore) {
18621 }
18622 assertNull(calibrator);
18623 }
18624
18625 @Test
18626 public void testConstructor149() throws WrongSizeException {
18627 final Matrix ba = generateBa();
18628 final double[] bias = ba.getBuffer();
18629 final double biasX = ba.getElementAtIndex(0);
18630 final double biasY = ba.getElementAtIndex(1);
18631 final double biasZ = ba.getElementAtIndex(2);
18632
18633 final Matrix ma = generateMaCommonAxis();
18634 final double sx = ma.getElementAt(0, 0);
18635 final double sy = ma.getElementAt(1, 1);
18636 final double sz = ma.getElementAt(2, 2);
18637 final double mxy = ma.getElementAt(0, 1);
18638 final double mxz = ma.getElementAt(0, 2);
18639 final double myx = ma.getElementAt(1, 0);
18640 final double myz = ma.getElementAt(1, 2);
18641 final double mzx = ma.getElementAt(2, 0);
18642 final double mzy = ma.getElementAt(2, 1);
18643
18644 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18645 final double latitude = Math.toRadians(
18646 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18647 final double longitude = Math.toRadians(
18648 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18649 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18650 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18651 final NEDVelocity nedVelocity = new NEDVelocity();
18652 final ECEFPosition ecefPosition = new ECEFPosition();
18653 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18654 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18655 ecefPosition, ecefVelocity);
18656
18657 KnownPositionAccelerometerCalibrator calibrator =
18658 new KnownPositionAccelerometerCalibrator(ecefPosition, ba, ma);
18659
18660
18661 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18662 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18663 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18664 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18665 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18666 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18667 final Acceleration bx2 = new Acceleration(0.0,
18668 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18669 calibrator.getInitialBiasXAsAcceleration(bx2);
18670 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18671 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18672 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18673 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18674 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18675 final Acceleration by2 = new Acceleration(0.0,
18676 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18677 calibrator.getInitialBiasYAsAcceleration(by2);
18678 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18679 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18680 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18681 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18682 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18683 final Acceleration bz2 = new Acceleration(0.0,
18684 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18685 calibrator.getInitialBiasZAsAcceleration(bz2);
18686 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18687 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18688 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18689 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18690 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18691 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18692 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18693 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18694 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18695 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18696 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18697 final double[] bias1 = calibrator.getInitialBias();
18698 assertArrayEquals(bias1, bias, 0.0);
18699 final double[] bias2 = new double[3];
18700 calibrator.getInitialBias(bias2);
18701 assertArrayEquals(bias1, bias2, 0.0);
18702 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18703 assertEquals(b1, ba);
18704 final Matrix b2 = new Matrix(3, 1);
18705 calibrator.getInitialBiasAsMatrix(b2);
18706 assertEquals(b1, b2);
18707 final Matrix ma1 = new Matrix(3, 3);
18708 ma1.setSubmatrix(0, 0,
18709 2, 2,
18710 new double[]{sx, myx, mzx,
18711 mxy, sy, mzy,
18712 mxz, myz, sz});
18713 assertEquals(calibrator.getInitialMa(), ma1);
18714 final Matrix ma2 = new Matrix(3, 3);
18715 calibrator.getInitialMa(ma2);
18716 assertEquals(ma1, ma2);
18717 assertSame(calibrator.getEcefPosition(), ecefPosition);
18718 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18719 final NEDPosition nedPosition1 = new NEDPosition();
18720 assertTrue(calibrator.getNedPosition(nedPosition1));
18721 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18722 assertNull(calibrator.getMeasurements());
18723 assertFalse(calibrator.isCommonAxisUsed());
18724 assertNull(calibrator.getListener());
18725 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18726 assertFalse(calibrator.isReady());
18727 assertFalse(calibrator.isRunning());
18728 assertNull(calibrator.getEstimatedBiases());
18729 assertFalse(calibrator.getEstimatedBiases(null));
18730 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18731 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18732 assertNull(calibrator.getEstimatedBiasFx());
18733 assertNull(calibrator.getEstimatedBiasFy());
18734 assertNull(calibrator.getEstimatedBiasFz());
18735 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18736 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18737 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18738 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18739 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18740 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18741 assertNull(calibrator.getEstimatedMa());
18742 assertNull(calibrator.getEstimatedSx());
18743 assertNull(calibrator.getEstimatedSy());
18744 assertNull(calibrator.getEstimatedSz());
18745 assertNull(calibrator.getEstimatedMxy());
18746 assertNull(calibrator.getEstimatedMxz());
18747 assertNull(calibrator.getEstimatedMyx());
18748 assertNull(calibrator.getEstimatedMyz());
18749 assertNull(calibrator.getEstimatedMzx());
18750 assertNull(calibrator.getEstimatedMzy());
18751 assertNull(calibrator.getEstimatedCovariance());
18752 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18753 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18754 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18755 assertNotNull(calibrator.getGroundTruthGravityNorm());
18756 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18757 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18758 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18759 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18760 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18761 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18762 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18763
18764
18765 calibrator = null;
18766 try {
18767 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18768 new Matrix(1, 1), ma);
18769 fail("IllegalArgumentException expected but not thrown");
18770 } catch (final IllegalArgumentException ignore) {
18771 }
18772 try {
18773 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18774 new Matrix(1, 3), ma);
18775 fail("IllegalArgumentException expected but not thrown");
18776 } catch (final IllegalArgumentException ignore) {
18777 }
18778 try {
18779 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18780 ba, new Matrix(1, 3));
18781 fail("IllegalArgumentException expected but not thrown");
18782 } catch (final IllegalArgumentException ignore) {
18783 }
18784 try {
18785 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18786 ba, new Matrix(3, 1));
18787 fail("IllegalArgumentException expected but not thrown");
18788 } catch (final IllegalArgumentException ignore) {
18789 }
18790 assertNull(calibrator);
18791 }
18792
18793 @Test
18794 public void testConstructor150() throws WrongSizeException {
18795 final Matrix ba = generateBa();
18796 final double[] bias = ba.getBuffer();
18797 final double biasX = ba.getElementAtIndex(0);
18798 final double biasY = ba.getElementAtIndex(1);
18799 final double biasZ = ba.getElementAtIndex(2);
18800
18801 final Matrix ma = generateMaCommonAxis();
18802 final double sx = ma.getElementAt(0, 0);
18803 final double sy = ma.getElementAt(1, 1);
18804 final double sz = ma.getElementAt(2, 2);
18805 final double mxy = ma.getElementAt(0, 1);
18806 final double mxz = ma.getElementAt(0, 2);
18807 final double myx = ma.getElementAt(1, 0);
18808 final double myz = ma.getElementAt(1, 2);
18809 final double mzx = ma.getElementAt(2, 0);
18810 final double mzy = ma.getElementAt(2, 1);
18811
18812 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18813 final double latitude = Math.toRadians(
18814 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18815 final double longitude = Math.toRadians(
18816 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18817 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18818 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18819 final NEDVelocity nedVelocity = new NEDVelocity();
18820 final ECEFPosition ecefPosition = new ECEFPosition();
18821 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18822 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18823 ecefPosition, ecefVelocity);
18824
18825 KnownPositionAccelerometerCalibrator calibrator =
18826 new KnownPositionAccelerometerCalibrator(ecefPosition,
18827 ba, ma, this);
18828
18829
18830 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18831 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18832 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18833 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18834 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18835 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18836 final Acceleration bx2 = new Acceleration(0.0,
18837 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18838 calibrator.getInitialBiasXAsAcceleration(bx2);
18839 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18840 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18841 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18842 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18843 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18844 final Acceleration by2 = new Acceleration(0.0,
18845 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18846 calibrator.getInitialBiasYAsAcceleration(by2);
18847 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18848 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18849 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18850 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18851 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18852 final Acceleration bz2 = new Acceleration(0.0,
18853 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18854 calibrator.getInitialBiasZAsAcceleration(bz2);
18855 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18856 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18857 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18858 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18859 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18860 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18861 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18862 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18863 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18864 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18865 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18866 final double[] bias1 = calibrator.getInitialBias();
18867 assertArrayEquals(bias1, bias, 0.0);
18868 final double[] bias2 = new double[3];
18869 calibrator.getInitialBias(bias2);
18870 assertArrayEquals(bias1, bias2, 0.0);
18871 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18872 assertEquals(b1, ba);
18873 final Matrix b2 = new Matrix(3, 1);
18874 calibrator.getInitialBiasAsMatrix(b2);
18875 assertEquals(b1, b2);
18876 final Matrix ma1 = new Matrix(3, 3);
18877 ma1.setSubmatrix(0, 0,
18878 2, 2,
18879 new double[]{sx, myx, mzx,
18880 mxy, sy, mzy,
18881 mxz, myz, sz});
18882 assertEquals(calibrator.getInitialMa(), ma1);
18883 final Matrix ma2 = new Matrix(3, 3);
18884 calibrator.getInitialMa(ma2);
18885 assertEquals(ma1, ma2);
18886 assertSame(calibrator.getEcefPosition(), ecefPosition);
18887 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18888 final NEDPosition nedPosition1 = new NEDPosition();
18889 assertTrue(calibrator.getNedPosition(nedPosition1));
18890 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18891 assertNull(calibrator.getMeasurements());
18892 assertFalse(calibrator.isCommonAxisUsed());
18893 assertSame(calibrator.getListener(), this);
18894 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18895 assertFalse(calibrator.isReady());
18896 assertFalse(calibrator.isRunning());
18897 assertNull(calibrator.getEstimatedBiases());
18898 assertFalse(calibrator.getEstimatedBiases(null));
18899 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18900 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18901 assertNull(calibrator.getEstimatedBiasFx());
18902 assertNull(calibrator.getEstimatedBiasFy());
18903 assertNull(calibrator.getEstimatedBiasFz());
18904 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18905 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18906 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18907 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18908 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18909 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18910 assertNull(calibrator.getEstimatedMa());
18911 assertNull(calibrator.getEstimatedSx());
18912 assertNull(calibrator.getEstimatedSy());
18913 assertNull(calibrator.getEstimatedSz());
18914 assertNull(calibrator.getEstimatedMxy());
18915 assertNull(calibrator.getEstimatedMxz());
18916 assertNull(calibrator.getEstimatedMyx());
18917 assertNull(calibrator.getEstimatedMyz());
18918 assertNull(calibrator.getEstimatedMzx());
18919 assertNull(calibrator.getEstimatedMzy());
18920 assertNull(calibrator.getEstimatedCovariance());
18921 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18922 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18923 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18924 assertNotNull(calibrator.getGroundTruthGravityNorm());
18925 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18926 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18927 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18928 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18929 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18930 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18931 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18932
18933
18934 calibrator = null;
18935 try {
18936 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18937 new Matrix(1, 1), ma, this);
18938 fail("IllegalArgumentException expected but not thrown");
18939 } catch (final IllegalArgumentException ignore) {
18940 }
18941 try {
18942 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18943 new Matrix(1, 3), ma, this);
18944 fail("IllegalArgumentException expected but not thrown");
18945 } catch (final IllegalArgumentException ignore) {
18946 }
18947 try {
18948 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18949 ba, new Matrix(1, 3), this);
18950 fail("IllegalArgumentException expected but not thrown");
18951 } catch (final IllegalArgumentException ignore) {
18952 }
18953 try {
18954 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18955 ba, new Matrix(3, 1), this);
18956 fail("IllegalArgumentException expected but not thrown");
18957 } catch (final IllegalArgumentException ignore) {
18958 }
18959 assertNull(calibrator);
18960 }
18961
18962 @Test
18963 public void testConstructor151() throws WrongSizeException {
18964 final Collection<StandardDeviationBodyKinematics> measurements =
18965 Collections.emptyList();
18966
18967 final Matrix ba = generateBa();
18968 final double[] bias = ba.getBuffer();
18969 final double biasX = ba.getElementAtIndex(0);
18970 final double biasY = ba.getElementAtIndex(1);
18971 final double biasZ = ba.getElementAtIndex(2);
18972
18973 final Matrix ma = generateMaCommonAxis();
18974 final double sx = ma.getElementAt(0, 0);
18975 final double sy = ma.getElementAt(1, 1);
18976 final double sz = ma.getElementAt(2, 2);
18977 final double mxy = ma.getElementAt(0, 1);
18978 final double mxz = ma.getElementAt(0, 2);
18979 final double myx = ma.getElementAt(1, 0);
18980 final double myz = ma.getElementAt(1, 2);
18981 final double mzx = ma.getElementAt(2, 0);
18982 final double mzy = ma.getElementAt(2, 1);
18983
18984 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18985 final double latitude = Math.toRadians(
18986 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18987 final double longitude = Math.toRadians(
18988 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18989 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18990 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18991 final NEDVelocity nedVelocity = new NEDVelocity();
18992 final ECEFPosition ecefPosition = new ECEFPosition();
18993 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18994 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18995 ecefPosition, ecefVelocity);
18996
18997 KnownPositionAccelerometerCalibrator calibrator =
18998 new KnownPositionAccelerometerCalibrator(ecefPosition,
18999 measurements, ba, ma);
19000
19001
19002 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19003 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19004 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19005 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19006 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19007 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19008 final Acceleration bx2 = new Acceleration(0.0,
19009 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19010 calibrator.getInitialBiasXAsAcceleration(bx2);
19011 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19012 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19013 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19014 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19015 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19016 final Acceleration by2 = new Acceleration(0.0,
19017 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19018 calibrator.getInitialBiasYAsAcceleration(by2);
19019 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19020 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19021 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19022 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19023 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19024 final Acceleration bz2 = new Acceleration(0.0,
19025 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19026 calibrator.getInitialBiasZAsAcceleration(bz2);
19027 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19028 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19029 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19030 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19031 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19032 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19033 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19034 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19035 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19036 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19037 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19038 final double[] bias1 = calibrator.getInitialBias();
19039 assertArrayEquals(bias1, bias, 0.0);
19040 final double[] bias2 = new double[3];
19041 calibrator.getInitialBias(bias2);
19042 assertArrayEquals(bias1, bias2, 0.0);
19043 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19044 assertEquals(b1, ba);
19045 final Matrix b2 = new Matrix(3, 1);
19046 calibrator.getInitialBiasAsMatrix(b2);
19047 assertEquals(b1, b2);
19048 final Matrix ma1 = new Matrix(3, 3);
19049 ma1.setSubmatrix(0, 0,
19050 2, 2,
19051 new double[]{sx, myx, mzx,
19052 mxy, sy, mzy,
19053 mxz, myz, sz});
19054 assertEquals(calibrator.getInitialMa(), ma1);
19055 final Matrix ma2 = new Matrix(3, 3);
19056 calibrator.getInitialMa(ma2);
19057 assertEquals(ma1, ma2);
19058 assertSame(calibrator.getEcefPosition(), ecefPosition);
19059 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19060 final NEDPosition nedPosition1 = new NEDPosition();
19061 assertTrue(calibrator.getNedPosition(nedPosition1));
19062 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19063 assertSame(calibrator.getMeasurements(), measurements);
19064 assertFalse(calibrator.isCommonAxisUsed());
19065 assertNull(calibrator.getListener());
19066 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19067 assertFalse(calibrator.isReady());
19068 assertFalse(calibrator.isRunning());
19069 assertNull(calibrator.getEstimatedBiases());
19070 assertFalse(calibrator.getEstimatedBiases(null));
19071 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19072 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19073 assertNull(calibrator.getEstimatedBiasFx());
19074 assertNull(calibrator.getEstimatedBiasFy());
19075 assertNull(calibrator.getEstimatedBiasFz());
19076 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19077 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19078 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19079 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19080 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19081 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19082 assertNull(calibrator.getEstimatedMa());
19083 assertNull(calibrator.getEstimatedSx());
19084 assertNull(calibrator.getEstimatedSy());
19085 assertNull(calibrator.getEstimatedSz());
19086 assertNull(calibrator.getEstimatedMxy());
19087 assertNull(calibrator.getEstimatedMxz());
19088 assertNull(calibrator.getEstimatedMyx());
19089 assertNull(calibrator.getEstimatedMyz());
19090 assertNull(calibrator.getEstimatedMzx());
19091 assertNull(calibrator.getEstimatedMzy());
19092 assertNull(calibrator.getEstimatedCovariance());
19093 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19094 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19095 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19096 assertNotNull(calibrator.getGroundTruthGravityNorm());
19097 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19098 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19099 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19100 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19101 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19102 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19103 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19104
19105
19106 calibrator = null;
19107 try {
19108 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19109 measurements, new Matrix(1, 1), ma);
19110 fail("IllegalArgumentException expected but not thrown");
19111 } catch (final IllegalArgumentException ignore) {
19112 }
19113 try {
19114 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19115 measurements, new Matrix(1, 3), ma);
19116 fail("IllegalArgumentException expected but not thrown");
19117 } catch (final IllegalArgumentException ignore) {
19118 }
19119 try {
19120 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19121 measurements, ba, new Matrix(1, 3));
19122 fail("IllegalArgumentException expected but not thrown");
19123 } catch (final IllegalArgumentException ignore) {
19124 }
19125 try {
19126 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19127 measurements, ba, new Matrix(3, 1));
19128 fail("IllegalArgumentException expected but not thrown");
19129 } catch (final IllegalArgumentException ignore) {
19130 }
19131 assertNull(calibrator);
19132 }
19133
19134 @Test
19135 public void testConstructor152() throws WrongSizeException {
19136 final Collection<StandardDeviationBodyKinematics> measurements =
19137 Collections.emptyList();
19138
19139 final Matrix ba = generateBa();
19140 final double[] bias = ba.getBuffer();
19141 final double biasX = ba.getElementAtIndex(0);
19142 final double biasY = ba.getElementAtIndex(1);
19143 final double biasZ = ba.getElementAtIndex(2);
19144
19145 final Matrix ma = generateMaCommonAxis();
19146 final double sx = ma.getElementAt(0, 0);
19147 final double sy = ma.getElementAt(1, 1);
19148 final double sz = ma.getElementAt(2, 2);
19149 final double mxy = ma.getElementAt(0, 1);
19150 final double mxz = ma.getElementAt(0, 2);
19151 final double myx = ma.getElementAt(1, 0);
19152 final double myz = ma.getElementAt(1, 2);
19153 final double mzx = ma.getElementAt(2, 0);
19154 final double mzy = ma.getElementAt(2, 1);
19155
19156 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19157 final double latitude = Math.toRadians(
19158 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19159 final double longitude = Math.toRadians(
19160 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19161 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19162 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19163 final NEDVelocity nedVelocity = new NEDVelocity();
19164 final ECEFPosition ecefPosition = new ECEFPosition();
19165 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19166 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19167 ecefPosition, ecefVelocity);
19168
19169 KnownPositionAccelerometerCalibrator calibrator =
19170 new KnownPositionAccelerometerCalibrator(ecefPosition,
19171 measurements, ba, ma, this);
19172
19173
19174 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19175 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19176 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19177 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19178 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19179 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19180 final Acceleration bx2 = new Acceleration(0.0,
19181 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19182 calibrator.getInitialBiasXAsAcceleration(bx2);
19183 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19184 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19185 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19186 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19187 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19188 final Acceleration by2 = new Acceleration(0.0,
19189 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19190 calibrator.getInitialBiasYAsAcceleration(by2);
19191 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19192 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19193 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19194 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19195 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19196 final Acceleration bz2 = new Acceleration(0.0,
19197 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19198 calibrator.getInitialBiasZAsAcceleration(bz2);
19199 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19200 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19201 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19202 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19203 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19204 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19205 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19206 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19207 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19208 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19209 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19210 final double[] bias1 = calibrator.getInitialBias();
19211 assertArrayEquals(bias1, bias, 0.0);
19212 final double[] bias2 = new double[3];
19213 calibrator.getInitialBias(bias2);
19214 assertArrayEquals(bias1, bias2, 0.0);
19215 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19216 assertEquals(b1, ba);
19217 final Matrix b2 = new Matrix(3, 1);
19218 calibrator.getInitialBiasAsMatrix(b2);
19219 assertEquals(b1, b2);
19220 final Matrix ma1 = new Matrix(3, 3);
19221 ma1.setSubmatrix(0, 0,
19222 2, 2,
19223 new double[]{sx, myx, mzx,
19224 mxy, sy, mzy,
19225 mxz, myz, sz});
19226 assertEquals(calibrator.getInitialMa(), ma1);
19227 final Matrix ma2 = new Matrix(3, 3);
19228 calibrator.getInitialMa(ma2);
19229 assertEquals(ma1, ma2);
19230 assertSame(calibrator.getEcefPosition(), ecefPosition);
19231 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19232 final NEDPosition nedPosition1 = new NEDPosition();
19233 assertTrue(calibrator.getNedPosition(nedPosition1));
19234 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19235 assertSame(calibrator.getMeasurements(), measurements);
19236 assertFalse(calibrator.isCommonAxisUsed());
19237 assertSame(calibrator.getListener(), this);
19238 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19239 assertFalse(calibrator.isReady());
19240 assertFalse(calibrator.isRunning());
19241 assertNull(calibrator.getEstimatedBiases());
19242 assertFalse(calibrator.getEstimatedBiases(null));
19243 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19244 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19245 assertNull(calibrator.getEstimatedBiasFx());
19246 assertNull(calibrator.getEstimatedBiasFy());
19247 assertNull(calibrator.getEstimatedBiasFz());
19248 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19249 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19250 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19251 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19252 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19253 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19254 assertNull(calibrator.getEstimatedMa());
19255 assertNull(calibrator.getEstimatedSx());
19256 assertNull(calibrator.getEstimatedSy());
19257 assertNull(calibrator.getEstimatedSz());
19258 assertNull(calibrator.getEstimatedMxy());
19259 assertNull(calibrator.getEstimatedMxz());
19260 assertNull(calibrator.getEstimatedMyx());
19261 assertNull(calibrator.getEstimatedMyz());
19262 assertNull(calibrator.getEstimatedMzx());
19263 assertNull(calibrator.getEstimatedMzy());
19264 assertNull(calibrator.getEstimatedCovariance());
19265 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19266 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19267 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19268 assertNotNull(calibrator.getGroundTruthGravityNorm());
19269 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19270 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19271 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19272 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19273 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19274 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19275 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19276
19277
19278 calibrator = null;
19279 try {
19280 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19281 measurements, new Matrix(1, 1), ma, this);
19282 fail("IllegalArgumentException expected but not thrown");
19283 } catch (final IllegalArgumentException ignore) {
19284 }
19285 try {
19286 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19287 measurements, new Matrix(1, 3), ma, this);
19288 fail("IllegalArgumentException expected but not thrown");
19289 } catch (final IllegalArgumentException ignore) {
19290 }
19291 try {
19292 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19293 measurements, ba, new Matrix(1, 3), this);
19294 fail("IllegalArgumentException expected but not thrown");
19295 } catch (final IllegalArgumentException ignore) {
19296 }
19297 try {
19298 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19299 measurements, ba, new Matrix(3, 1), this);
19300 fail("IllegalArgumentException expected but not thrown");
19301 } catch (final IllegalArgumentException ignore) {
19302 }
19303 assertNull(calibrator);
19304 }
19305
19306 @Test
19307 public void testConstructor153() throws WrongSizeException {
19308 final Matrix ba = generateBa();
19309 final double[] bias = ba.getBuffer();
19310 final double biasX = ba.getElementAtIndex(0);
19311 final double biasY = ba.getElementAtIndex(1);
19312 final double biasZ = ba.getElementAtIndex(2);
19313
19314 final Matrix ma = generateMaCommonAxis();
19315 final double sx = ma.getElementAt(0, 0);
19316 final double sy = ma.getElementAt(1, 1);
19317 final double sz = ma.getElementAt(2, 2);
19318 final double mxy = ma.getElementAt(0, 1);
19319 final double mxz = ma.getElementAt(0, 2);
19320 final double myx = ma.getElementAt(1, 0);
19321 final double myz = ma.getElementAt(1, 2);
19322 final double mzx = ma.getElementAt(2, 0);
19323 final double mzy = ma.getElementAt(2, 1);
19324
19325 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19326 final double latitude = Math.toRadians(
19327 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19328 final double longitude = Math.toRadians(
19329 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19330 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19331 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19332 final NEDVelocity nedVelocity = new NEDVelocity();
19333 final ECEFPosition ecefPosition = new ECEFPosition();
19334 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19335 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19336 ecefPosition, ecefVelocity);
19337
19338 KnownPositionAccelerometerCalibrator calibrator =
19339 new KnownPositionAccelerometerCalibrator(ecefPosition,
19340 true, ba, ma);
19341
19342
19343 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19344 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19345 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19346 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19347 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19348 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19349 final Acceleration bx2 = new Acceleration(0.0,
19350 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19351 calibrator.getInitialBiasXAsAcceleration(bx2);
19352 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19353 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19354 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19355 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19356 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19357 final Acceleration by2 = new Acceleration(0.0,
19358 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19359 calibrator.getInitialBiasYAsAcceleration(by2);
19360 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19361 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19362 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19363 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19364 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19365 final Acceleration bz2 = new Acceleration(0.0,
19366 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19367 calibrator.getInitialBiasZAsAcceleration(bz2);
19368 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19369 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19370 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19371 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19372 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19373 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19374 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19375 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19376 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19377 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19378 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19379 final double[] bias1 = calibrator.getInitialBias();
19380 assertArrayEquals(bias1, bias, 0.0);
19381 final double[] bias2 = new double[3];
19382 calibrator.getInitialBias(bias2);
19383 assertArrayEquals(bias1, bias2, 0.0);
19384 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19385 assertEquals(b1, ba);
19386 final Matrix b2 = new Matrix(3, 1);
19387 calibrator.getInitialBiasAsMatrix(b2);
19388 assertEquals(b1, b2);
19389 final Matrix ma1 = new Matrix(3, 3);
19390 ma1.setSubmatrix(0, 0,
19391 2, 2,
19392 new double[]{sx, myx, mzx,
19393 mxy, sy, mzy,
19394 mxz, myz, sz});
19395 assertEquals(calibrator.getInitialMa(), ma1);
19396 final Matrix ma2 = new Matrix(3, 3);
19397 calibrator.getInitialMa(ma2);
19398 assertEquals(ma1, ma2);
19399 assertSame(calibrator.getEcefPosition(), ecefPosition);
19400 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19401 final NEDPosition nedPosition1 = new NEDPosition();
19402 assertTrue(calibrator.getNedPosition(nedPosition1));
19403 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19404 assertNull(calibrator.getMeasurements());
19405 assertTrue(calibrator.isCommonAxisUsed());
19406 assertNull(calibrator.getListener());
19407 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19408 assertFalse(calibrator.isReady());
19409 assertFalse(calibrator.isRunning());
19410 assertNull(calibrator.getEstimatedBiases());
19411 assertFalse(calibrator.getEstimatedBiases(null));
19412 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19413 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19414 assertNull(calibrator.getEstimatedBiasFx());
19415 assertNull(calibrator.getEstimatedBiasFy());
19416 assertNull(calibrator.getEstimatedBiasFz());
19417 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19418 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19419 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19420 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19421 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19422 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19423 assertNull(calibrator.getEstimatedMa());
19424 assertNull(calibrator.getEstimatedSx());
19425 assertNull(calibrator.getEstimatedSy());
19426 assertNull(calibrator.getEstimatedSz());
19427 assertNull(calibrator.getEstimatedMxy());
19428 assertNull(calibrator.getEstimatedMxz());
19429 assertNull(calibrator.getEstimatedMyx());
19430 assertNull(calibrator.getEstimatedMyz());
19431 assertNull(calibrator.getEstimatedMzx());
19432 assertNull(calibrator.getEstimatedMzy());
19433 assertNull(calibrator.getEstimatedCovariance());
19434 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19435 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19436 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19437 assertNotNull(calibrator.getGroundTruthGravityNorm());
19438 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19439 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19440 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19441 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19442 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19443 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19444 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19445
19446
19447 calibrator = null;
19448 try {
19449 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19450 true, new Matrix(1, 1), ma);
19451 fail("IllegalArgumentException expected but not thrown");
19452 } catch (final IllegalArgumentException ignore) {
19453 }
19454 try {
19455 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19456 true, new Matrix(1, 3), ma);
19457 fail("IllegalArgumentException expected but not thrown");
19458 } catch (final IllegalArgumentException ignore) {
19459 }
19460 try {
19461 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19462 true, ba, new Matrix(1, 3));
19463 fail("IllegalArgumentException expected but not thrown");
19464 } catch (final IllegalArgumentException ignore) {
19465 }
19466 try {
19467 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19468 true, ba, new Matrix(3, 1));
19469 fail("IllegalArgumentException expected but not thrown");
19470 } catch (final IllegalArgumentException ignore) {
19471 }
19472 assertNull(calibrator);
19473 }
19474
19475 @Test
19476 public void testConstructor154() throws WrongSizeException {
19477 final Matrix ba = generateBa();
19478 final double[] bias = ba.getBuffer();
19479 final double biasX = ba.getElementAtIndex(0);
19480 final double biasY = ba.getElementAtIndex(1);
19481 final double biasZ = ba.getElementAtIndex(2);
19482
19483 final Matrix ma = generateMaCommonAxis();
19484 final double sx = ma.getElementAt(0, 0);
19485 final double sy = ma.getElementAt(1, 1);
19486 final double sz = ma.getElementAt(2, 2);
19487 final double mxy = ma.getElementAt(0, 1);
19488 final double mxz = ma.getElementAt(0, 2);
19489 final double myx = ma.getElementAt(1, 0);
19490 final double myz = ma.getElementAt(1, 2);
19491 final double mzx = ma.getElementAt(2, 0);
19492 final double mzy = ma.getElementAt(2, 1);
19493
19494 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19495 final double latitude = Math.toRadians(
19496 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19497 final double longitude = Math.toRadians(
19498 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19499 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19500 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19501 final NEDVelocity nedVelocity = new NEDVelocity();
19502 final ECEFPosition ecefPosition = new ECEFPosition();
19503 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19504 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19505 ecefPosition, ecefVelocity);
19506
19507 KnownPositionAccelerometerCalibrator calibrator =
19508 new KnownPositionAccelerometerCalibrator(ecefPosition,
19509 true, ba, ma, this);
19510
19511
19512 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19513 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19514 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19515 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19516 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19517 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19518 final Acceleration bx2 = new Acceleration(0.0,
19519 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19520 calibrator.getInitialBiasXAsAcceleration(bx2);
19521 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19522 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19523 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19524 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19525 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19526 final Acceleration by2 = new Acceleration(0.0,
19527 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19528 calibrator.getInitialBiasYAsAcceleration(by2);
19529 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19530 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19531 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19532 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19533 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19534 final Acceleration bz2 = new Acceleration(0.0,
19535 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19536 calibrator.getInitialBiasZAsAcceleration(bz2);
19537 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19538 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19539 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19540 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19541 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19542 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19543 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19544 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19545 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19546 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19547 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19548 final double[] bias1 = calibrator.getInitialBias();
19549 assertArrayEquals(bias1, bias, 0.0);
19550 final double[] bias2 = new double[3];
19551 calibrator.getInitialBias(bias2);
19552 assertArrayEquals(bias1, bias2, 0.0);
19553 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19554 assertEquals(b1, ba);
19555 final Matrix b2 = new Matrix(3, 1);
19556 calibrator.getInitialBiasAsMatrix(b2);
19557 assertEquals(b1, b2);
19558 final Matrix ma1 = new Matrix(3, 3);
19559 ma1.setSubmatrix(0, 0,
19560 2, 2,
19561 new double[]{sx, myx, mzx,
19562 mxy, sy, mzy,
19563 mxz, myz, sz});
19564 assertEquals(calibrator.getInitialMa(), ma1);
19565 final Matrix ma2 = new Matrix(3, 3);
19566 calibrator.getInitialMa(ma2);
19567 assertEquals(ma1, ma2);
19568 assertSame(calibrator.getEcefPosition(), ecefPosition);
19569 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19570 final NEDPosition nedPosition1 = new NEDPosition();
19571 assertTrue(calibrator.getNedPosition(nedPosition1));
19572 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19573 assertNull(calibrator.getMeasurements());
19574 assertTrue(calibrator.isCommonAxisUsed());
19575 assertSame(calibrator.getListener(), this);
19576 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19577 assertFalse(calibrator.isReady());
19578 assertFalse(calibrator.isRunning());
19579 assertNull(calibrator.getEstimatedBiases());
19580 assertFalse(calibrator.getEstimatedBiases(null));
19581 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19582 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19583 assertNull(calibrator.getEstimatedBiasFx());
19584 assertNull(calibrator.getEstimatedBiasFy());
19585 assertNull(calibrator.getEstimatedBiasFz());
19586 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19587 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19588 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19589 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19590 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19591 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19592 assertNull(calibrator.getEstimatedMa());
19593 assertNull(calibrator.getEstimatedSx());
19594 assertNull(calibrator.getEstimatedSy());
19595 assertNull(calibrator.getEstimatedSz());
19596 assertNull(calibrator.getEstimatedMxy());
19597 assertNull(calibrator.getEstimatedMxz());
19598 assertNull(calibrator.getEstimatedMyx());
19599 assertNull(calibrator.getEstimatedMyz());
19600 assertNull(calibrator.getEstimatedMzx());
19601 assertNull(calibrator.getEstimatedMzy());
19602 assertNull(calibrator.getEstimatedCovariance());
19603 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19604 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19605 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19606 assertNotNull(calibrator.getGroundTruthGravityNorm());
19607 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19608 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19609 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19610 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19611 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19612 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19613 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19614
19615
19616 calibrator = null;
19617 try {
19618 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19619 true, new Matrix(1, 1), ma, this);
19620 fail("IllegalArgumentException expected but not thrown");
19621 } catch (final IllegalArgumentException ignore) {
19622 }
19623 try {
19624 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19625 true, new Matrix(1, 3), ma, this);
19626 fail("IllegalArgumentException expected but not thrown");
19627 } catch (final IllegalArgumentException ignore) {
19628 }
19629 try {
19630 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19631 true, ba, new Matrix(1, 3), this);
19632 fail("IllegalArgumentException expected but not thrown");
19633 } catch (final IllegalArgumentException ignore) {
19634 }
19635 try {
19636 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19637 true, ba, new Matrix(3, 1), this);
19638 fail("IllegalArgumentException expected but not thrown");
19639 } catch (final IllegalArgumentException ignore) {
19640 }
19641 assertNull(calibrator);
19642 }
19643
19644 @Test
19645 public void testConstructor155() throws WrongSizeException {
19646 final Collection<StandardDeviationBodyKinematics> measurements =
19647 Collections.emptyList();
19648
19649 final Matrix ba = generateBa();
19650 final double[] bias = ba.getBuffer();
19651 final double biasX = ba.getElementAtIndex(0);
19652 final double biasY = ba.getElementAtIndex(1);
19653 final double biasZ = ba.getElementAtIndex(2);
19654
19655 final Matrix ma = generateMaCommonAxis();
19656 final double sx = ma.getElementAt(0, 0);
19657 final double sy = ma.getElementAt(1, 1);
19658 final double sz = ma.getElementAt(2, 2);
19659 final double mxy = ma.getElementAt(0, 1);
19660 final double mxz = ma.getElementAt(0, 2);
19661 final double myx = ma.getElementAt(1, 0);
19662 final double myz = ma.getElementAt(1, 2);
19663 final double mzx = ma.getElementAt(2, 0);
19664 final double mzy = ma.getElementAt(2, 1);
19665
19666 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19667 final double latitude = Math.toRadians(
19668 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19669 final double longitude = Math.toRadians(
19670 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19671 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19672 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19673 final NEDVelocity nedVelocity = new NEDVelocity();
19674 final ECEFPosition ecefPosition = new ECEFPosition();
19675 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19676 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19677 ecefPosition, ecefVelocity);
19678
19679 KnownPositionAccelerometerCalibrator calibrator =
19680 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19681 true, ba, ma);
19682
19683
19684 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19685 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19686 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19687 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19688 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19689 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19690 final Acceleration bx2 = new Acceleration(0.0,
19691 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19692 calibrator.getInitialBiasXAsAcceleration(bx2);
19693 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19694 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19695 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19696 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19697 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19698 final Acceleration by2 = new Acceleration(0.0,
19699 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19700 calibrator.getInitialBiasYAsAcceleration(by2);
19701 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19702 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19703 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19704 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19705 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19706 final Acceleration bz2 = new Acceleration(0.0,
19707 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19708 calibrator.getInitialBiasZAsAcceleration(bz2);
19709 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19710 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19711 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19712 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19713 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19714 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19715 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19716 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19717 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19718 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19719 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19720 final double[] bias1 = calibrator.getInitialBias();
19721 assertArrayEquals(bias1, bias, 0.0);
19722 final double[] bias2 = new double[3];
19723 calibrator.getInitialBias(bias2);
19724 assertArrayEquals(bias1, bias2, 0.0);
19725 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19726 assertEquals(b1, ba);
19727 final Matrix b2 = new Matrix(3, 1);
19728 calibrator.getInitialBiasAsMatrix(b2);
19729 assertEquals(b1, b2);
19730 final Matrix ma1 = new Matrix(3, 3);
19731 ma1.setSubmatrix(0, 0,
19732 2, 2,
19733 new double[]{sx, myx, mzx,
19734 mxy, sy, mzy,
19735 mxz, myz, sz});
19736 assertEquals(calibrator.getInitialMa(), ma1);
19737 final Matrix ma2 = new Matrix(3, 3);
19738 calibrator.getInitialMa(ma2);
19739 assertEquals(ma1, ma2);
19740 assertSame(calibrator.getEcefPosition(), ecefPosition);
19741 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19742 final NEDPosition nedPosition1 = new NEDPosition();
19743 assertTrue(calibrator.getNedPosition(nedPosition1));
19744 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19745 assertSame(calibrator.getMeasurements(), measurements);
19746 assertTrue(calibrator.isCommonAxisUsed());
19747 assertNull(calibrator.getListener());
19748 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19749 assertFalse(calibrator.isReady());
19750 assertFalse(calibrator.isRunning());
19751 assertNull(calibrator.getEstimatedBiases());
19752 assertFalse(calibrator.getEstimatedBiases(null));
19753 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19754 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19755 assertNull(calibrator.getEstimatedBiasFx());
19756 assertNull(calibrator.getEstimatedBiasFy());
19757 assertNull(calibrator.getEstimatedBiasFz());
19758 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19759 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19760 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19761 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19762 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19763 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19764 assertNull(calibrator.getEstimatedMa());
19765 assertNull(calibrator.getEstimatedSx());
19766 assertNull(calibrator.getEstimatedSy());
19767 assertNull(calibrator.getEstimatedSz());
19768 assertNull(calibrator.getEstimatedMxy());
19769 assertNull(calibrator.getEstimatedMxz());
19770 assertNull(calibrator.getEstimatedMyx());
19771 assertNull(calibrator.getEstimatedMyz());
19772 assertNull(calibrator.getEstimatedMzx());
19773 assertNull(calibrator.getEstimatedMzy());
19774 assertNull(calibrator.getEstimatedCovariance());
19775 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19776 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19777 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19778 assertNotNull(calibrator.getGroundTruthGravityNorm());
19779 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19780 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19781 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19782 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19783 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19784 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19785 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19786
19787
19788 calibrator = null;
19789 try {
19790 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19791 measurements, true,
19792 new Matrix(1, 1), ma);
19793 fail("IllegalArgumentException expected but not thrown");
19794 } catch (final IllegalArgumentException ignore) {
19795 }
19796 try {
19797 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19798 measurements, true,
19799 new Matrix(1, 3), ma);
19800 fail("IllegalArgumentException expected but not thrown");
19801 } catch (final IllegalArgumentException ignore) {
19802 }
19803 try {
19804 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19805 measurements, true,
19806 ba, new Matrix(1, 3));
19807 fail("IllegalArgumentException expected but not thrown");
19808 } catch (final IllegalArgumentException ignore) {
19809 }
19810 try {
19811 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19812 measurements, true,
19813 ba, new Matrix(3, 1));
19814 fail("IllegalArgumentException expected but not thrown");
19815 } catch (final IllegalArgumentException ignore) {
19816 }
19817 assertNull(calibrator);
19818 }
19819
19820 @Test
19821 public void testConstructor156() throws WrongSizeException {
19822 final Collection<StandardDeviationBodyKinematics> measurements =
19823 Collections.emptyList();
19824
19825 final Matrix ba = generateBa();
19826 final double[] bias = ba.getBuffer();
19827 final double biasX = ba.getElementAtIndex(0);
19828 final double biasY = ba.getElementAtIndex(1);
19829 final double biasZ = ba.getElementAtIndex(2);
19830
19831 final Matrix ma = generateMaCommonAxis();
19832 final double sx = ma.getElementAt(0, 0);
19833 final double sy = ma.getElementAt(1, 1);
19834 final double sz = ma.getElementAt(2, 2);
19835 final double mxy = ma.getElementAt(0, 1);
19836 final double mxz = ma.getElementAt(0, 2);
19837 final double myx = ma.getElementAt(1, 0);
19838 final double myz = ma.getElementAt(1, 2);
19839 final double mzx = ma.getElementAt(2, 0);
19840 final double mzy = ma.getElementAt(2, 1);
19841
19842 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19843 final double latitude = Math.toRadians(
19844 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19845 final double longitude = Math.toRadians(
19846 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19847 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19848 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19849 final NEDVelocity nedVelocity = new NEDVelocity();
19850 final ECEFPosition ecefPosition = new ECEFPosition();
19851 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19852 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19853 ecefPosition, ecefVelocity);
19854
19855 KnownPositionAccelerometerCalibrator calibrator =
19856 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19857 true, ba, ma, this);
19858
19859
19860 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19861 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19862 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19863 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19864 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19865 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19866 final Acceleration bx2 = new Acceleration(0.0,
19867 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19868 calibrator.getInitialBiasXAsAcceleration(bx2);
19869 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19870 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19871 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19872 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19873 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19874 final Acceleration by2 = new Acceleration(0.0,
19875 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19876 calibrator.getInitialBiasYAsAcceleration(by2);
19877 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19878 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19879 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19880 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19881 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19882 final Acceleration bz2 = new Acceleration(0.0,
19883 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19884 calibrator.getInitialBiasZAsAcceleration(bz2);
19885 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19886 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19887 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19888 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19889 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19890 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19891 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19892 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19893 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19894 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19895 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19896 final double[] bias1 = calibrator.getInitialBias();
19897 assertArrayEquals(bias1, bias, 0.0);
19898 final double[] bias2 = new double[3];
19899 calibrator.getInitialBias(bias2);
19900 assertArrayEquals(bias1, bias2, 0.0);
19901 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19902 assertEquals(b1, ba);
19903 final Matrix b2 = new Matrix(3, 1);
19904 calibrator.getInitialBiasAsMatrix(b2);
19905 assertEquals(b1, b2);
19906 final Matrix ma1 = new Matrix(3, 3);
19907 ma1.setSubmatrix(0, 0,
19908 2, 2,
19909 new double[]{sx, myx, mzx,
19910 mxy, sy, mzy,
19911 mxz, myz, sz});
19912 assertEquals(calibrator.getInitialMa(), ma1);
19913 final Matrix ma2 = new Matrix(3, 3);
19914 calibrator.getInitialMa(ma2);
19915 assertEquals(ma1, ma2);
19916 assertSame(calibrator.getEcefPosition(), ecefPosition);
19917 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19918 final NEDPosition nedPosition1 = new NEDPosition();
19919 assertTrue(calibrator.getNedPosition(nedPosition1));
19920 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19921 assertSame(calibrator.getMeasurements(), measurements);
19922 assertTrue(calibrator.isCommonAxisUsed());
19923 assertSame(calibrator.getListener(), this);
19924 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19925 assertFalse(calibrator.isReady());
19926 assertFalse(calibrator.isRunning());
19927 assertNull(calibrator.getEstimatedBiases());
19928 assertFalse(calibrator.getEstimatedBiases(null));
19929 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19930 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19931 assertNull(calibrator.getEstimatedBiasFx());
19932 assertNull(calibrator.getEstimatedBiasFy());
19933 assertNull(calibrator.getEstimatedBiasFz());
19934 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19935 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19936 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19937 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19938 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19939 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19940 assertNull(calibrator.getEstimatedMa());
19941 assertNull(calibrator.getEstimatedSx());
19942 assertNull(calibrator.getEstimatedSy());
19943 assertNull(calibrator.getEstimatedSz());
19944 assertNull(calibrator.getEstimatedMxy());
19945 assertNull(calibrator.getEstimatedMxz());
19946 assertNull(calibrator.getEstimatedMyx());
19947 assertNull(calibrator.getEstimatedMyz());
19948 assertNull(calibrator.getEstimatedMzx());
19949 assertNull(calibrator.getEstimatedMzy());
19950 assertNull(calibrator.getEstimatedCovariance());
19951 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19952 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19953 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19954 assertNotNull(calibrator.getGroundTruthGravityNorm());
19955 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19956 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19957 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19958 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19959 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19960 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19961 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19962
19963
19964 calibrator = null;
19965 try {
19966 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19967 measurements, true,
19968 new Matrix(1, 1), ma, this);
19969 fail("IllegalArgumentException expected but not thrown");
19970 } catch (final IllegalArgumentException ignore) {
19971 }
19972 try {
19973 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19974 measurements, true,
19975 new Matrix(1, 3), ma, this);
19976 fail("IllegalArgumentException expected but not thrown");
19977 } catch (final IllegalArgumentException ignore) {
19978 }
19979 try {
19980 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19981 measurements, true,
19982 ba, new Matrix(1, 3), this);
19983 fail("IllegalArgumentException expected but not thrown");
19984 } catch (final IllegalArgumentException ignore) {
19985 }
19986 try {
19987 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19988 measurements, true,
19989 ba, new Matrix(3, 1), this);
19990 fail("IllegalArgumentException expected but not thrown");
19991 } catch (final IllegalArgumentException ignore) {
19992 }
19993 assertNull(calibrator);
19994 }
19995
19996 @Test
19997 public void testConstructor157() throws WrongSizeException {
19998 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19999 final double latitude = Math.toRadians(
20000 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20001 final double longitude = Math.toRadians(
20002 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20003 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20004 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20005 final NEDVelocity nedVelocity = new NEDVelocity();
20006 final ECEFPosition ecefPosition = new ECEFPosition();
20007 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20008 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20009 ecefPosition, ecefVelocity);
20010
20011 final KnownPositionAccelerometerCalibrator calibrator =
20012 new KnownPositionAccelerometerCalibrator(nedPosition);
20013
20014
20015 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20016 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20017 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20018 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20019 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20020 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20021 final Acceleration bx2 = new Acceleration(0.0,
20022 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20023 calibrator.getInitialBiasXAsAcceleration(bx2);
20024 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20025 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20026 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20027 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20028 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20029 final Acceleration by2 = new Acceleration(0.0,
20030 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20031 calibrator.getInitialBiasYAsAcceleration(by2);
20032 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20033 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20034 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20035 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20036 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20037 final Acceleration bz2 = new Acceleration(0.0,
20038 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20039 calibrator.getInitialBiasZAsAcceleration(bz2);
20040 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20041 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20042 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20043 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20044 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20045 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20046 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20047 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20048 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20049 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20050 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20051 final double[] bias1 = calibrator.getInitialBias();
20052 assertArrayEquals(bias1, new double[3], 0.0);
20053 final double[] bias2 = new double[3];
20054 calibrator.getInitialBias(bias2);
20055 assertArrayEquals(bias1, bias2, 0.0);
20056 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20057 assertEquals(b1, new Matrix(3, 1));
20058 final Matrix b2 = new Matrix(3, 1);
20059 calibrator.getInitialBiasAsMatrix(b2);
20060 assertEquals(b1, b2);
20061 final Matrix ma1 = calibrator.getInitialMa();
20062 assertEquals(ma1, new Matrix(3, 3));
20063 final Matrix ma2 = new Matrix(3, 3);
20064 calibrator.getInitialMa(ma2);
20065 assertEquals(ma1, ma2);
20066 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20067 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20068 final NEDPosition nedPosition1 = new NEDPosition();
20069 assertTrue(calibrator.getNedPosition(nedPosition1));
20070 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20071 assertNull(calibrator.getMeasurements());
20072 assertFalse(calibrator.isCommonAxisUsed());
20073 assertNull(calibrator.getListener());
20074 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20075 assertFalse(calibrator.isReady());
20076 assertFalse(calibrator.isRunning());
20077 assertNull(calibrator.getEstimatedBiases());
20078 assertFalse(calibrator.getEstimatedBiases(null));
20079 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20080 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20081 assertNull(calibrator.getEstimatedBiasFx());
20082 assertNull(calibrator.getEstimatedBiasFy());
20083 assertNull(calibrator.getEstimatedBiasFz());
20084 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20085 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20086 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20087 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20088 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20089 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20090 assertNull(calibrator.getEstimatedMa());
20091 assertNull(calibrator.getEstimatedSx());
20092 assertNull(calibrator.getEstimatedSy());
20093 assertNull(calibrator.getEstimatedSz());
20094 assertNull(calibrator.getEstimatedMxy());
20095 assertNull(calibrator.getEstimatedMxz());
20096 assertNull(calibrator.getEstimatedMyx());
20097 assertNull(calibrator.getEstimatedMyz());
20098 assertNull(calibrator.getEstimatedMzx());
20099 assertNull(calibrator.getEstimatedMzy());
20100 assertNull(calibrator.getEstimatedCovariance());
20101 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20102 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20103 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20104 assertNotNull(calibrator.getGroundTruthGravityNorm());
20105 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20106 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20107 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20108 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20109 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20110 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20111 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20112 }
20113
20114 @Test
20115 public void testConstructor158() throws WrongSizeException {
20116 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20117 final double latitude = Math.toRadians(
20118 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20119 final double longitude = Math.toRadians(
20120 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20121 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20122 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20123 final NEDVelocity nedVelocity = new NEDVelocity();
20124 final ECEFPosition ecefPosition = new ECEFPosition();
20125 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20126 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20127 ecefPosition, ecefVelocity);
20128
20129 final KnownPositionAccelerometerCalibrator calibrator =
20130 new KnownPositionAccelerometerCalibrator(nedPosition, this);
20131
20132
20133 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20134 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20135 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20136 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20137 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20138 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20139 final Acceleration bx2 = new Acceleration(0.0,
20140 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20141 calibrator.getInitialBiasXAsAcceleration(bx2);
20142 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20143 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20144 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20145 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20146 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20147 final Acceleration by2 = new Acceleration(0.0,
20148 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20149 calibrator.getInitialBiasYAsAcceleration(by2);
20150 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20151 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20152 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20153 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20154 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20155 final Acceleration bz2 = new Acceleration(0.0,
20156 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20157 calibrator.getInitialBiasZAsAcceleration(bz2);
20158 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20159 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20160 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20161 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20162 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20163 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20164 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20165 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20166 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20167 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20168 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20169 final double[] bias1 = calibrator.getInitialBias();
20170 assertArrayEquals(bias1, new double[3], 0.0);
20171 final double[] bias2 = new double[3];
20172 calibrator.getInitialBias(bias2);
20173 assertArrayEquals(bias1, bias2, 0.0);
20174 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20175 assertEquals(b1, new Matrix(3, 1));
20176 final Matrix b2 = new Matrix(3, 1);
20177 calibrator.getInitialBiasAsMatrix(b2);
20178 assertEquals(b1, b2);
20179 final Matrix ma1 = calibrator.getInitialMa();
20180 assertEquals(ma1, new Matrix(3, 3));
20181 final Matrix ma2 = new Matrix(3, 3);
20182 calibrator.getInitialMa(ma2);
20183 assertEquals(ma1, ma2);
20184 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20185 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20186 final NEDPosition nedPosition1 = new NEDPosition();
20187 assertTrue(calibrator.getNedPosition(nedPosition1));
20188 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20189 assertNull(calibrator.getMeasurements());
20190 assertFalse(calibrator.isCommonAxisUsed());
20191 assertSame(calibrator.getListener(), this);
20192 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20193 assertFalse(calibrator.isReady());
20194 assertFalse(calibrator.isRunning());
20195 assertNull(calibrator.getEstimatedBiases());
20196 assertFalse(calibrator.getEstimatedBiases(null));
20197 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20198 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20199 assertNull(calibrator.getEstimatedBiasFx());
20200 assertNull(calibrator.getEstimatedBiasFy());
20201 assertNull(calibrator.getEstimatedBiasFz());
20202 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20203 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20204 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20205 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20206 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20207 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20208 assertNull(calibrator.getEstimatedMa());
20209 assertNull(calibrator.getEstimatedSx());
20210 assertNull(calibrator.getEstimatedSy());
20211 assertNull(calibrator.getEstimatedSz());
20212 assertNull(calibrator.getEstimatedMxy());
20213 assertNull(calibrator.getEstimatedMxz());
20214 assertNull(calibrator.getEstimatedMyx());
20215 assertNull(calibrator.getEstimatedMyz());
20216 assertNull(calibrator.getEstimatedMzx());
20217 assertNull(calibrator.getEstimatedMzy());
20218 assertNull(calibrator.getEstimatedCovariance());
20219 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20220 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20221 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20222 assertNotNull(calibrator.getGroundTruthGravityNorm());
20223 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20224 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20225 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20226 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20227 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20228 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20229 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20230 }
20231
20232 @Test
20233 public void testConstructor159() throws WrongSizeException {
20234 final Collection<StandardDeviationBodyKinematics> measurements =
20235 Collections.emptyList();
20236
20237 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20238 final double latitude = Math.toRadians(
20239 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20240 final double longitude = Math.toRadians(
20241 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20242 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20243 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20244 final NEDVelocity nedVelocity = new NEDVelocity();
20245 final ECEFPosition ecefPosition = new ECEFPosition();
20246 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20247 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20248 ecefPosition, ecefVelocity);
20249
20250 final KnownPositionAccelerometerCalibrator calibrator =
20251 new KnownPositionAccelerometerCalibrator(nedPosition, measurements);
20252
20253
20254 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20255 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20256 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20257 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20258 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20259 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20260 final Acceleration bx2 = new Acceleration(0.0,
20261 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20262 calibrator.getInitialBiasXAsAcceleration(bx2);
20263 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20264 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20265 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20266 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20267 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20268 final Acceleration by2 = new Acceleration(0.0,
20269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20270 calibrator.getInitialBiasYAsAcceleration(by2);
20271 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20272 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20273 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20274 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20275 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20276 final Acceleration bz2 = new Acceleration(0.0,
20277 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20278 calibrator.getInitialBiasZAsAcceleration(bz2);
20279 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20280 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20281 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20282 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20283 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20284 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20285 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20286 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20287 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20288 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20289 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20290 final double[] bias1 = calibrator.getInitialBias();
20291 assertArrayEquals(bias1, new double[3], 0.0);
20292 final double[] bias2 = new double[3];
20293 calibrator.getInitialBias(bias2);
20294 assertArrayEquals(bias1, bias2, 0.0);
20295 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20296 assertEquals(b1, new Matrix(3, 1));
20297 final Matrix b2 = new Matrix(3, 1);
20298 calibrator.getInitialBiasAsMatrix(b2);
20299 assertEquals(b1, b2);
20300 final Matrix ma1 = calibrator.getInitialMa();
20301 assertEquals(ma1, new Matrix(3, 3));
20302 final Matrix ma2 = new Matrix(3, 3);
20303 calibrator.getInitialMa(ma2);
20304 assertEquals(ma1, ma2);
20305 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20306 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20307 final NEDPosition nedPosition1 = new NEDPosition();
20308 assertTrue(calibrator.getNedPosition(nedPosition1));
20309 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20310 assertSame(calibrator.getMeasurements(), measurements);
20311 assertFalse(calibrator.isCommonAxisUsed());
20312 assertNull(calibrator.getListener());
20313 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20314 assertFalse(calibrator.isReady());
20315 assertFalse(calibrator.isRunning());
20316 assertNull(calibrator.getEstimatedBiases());
20317 assertFalse(calibrator.getEstimatedBiases(null));
20318 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20319 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20320 assertNull(calibrator.getEstimatedBiasFx());
20321 assertNull(calibrator.getEstimatedBiasFy());
20322 assertNull(calibrator.getEstimatedBiasFz());
20323 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20324 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20325 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20326 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20327 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20328 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20329 assertNull(calibrator.getEstimatedMa());
20330 assertNull(calibrator.getEstimatedSx());
20331 assertNull(calibrator.getEstimatedSy());
20332 assertNull(calibrator.getEstimatedSz());
20333 assertNull(calibrator.getEstimatedMxy());
20334 assertNull(calibrator.getEstimatedMxz());
20335 assertNull(calibrator.getEstimatedMyx());
20336 assertNull(calibrator.getEstimatedMyz());
20337 assertNull(calibrator.getEstimatedMzx());
20338 assertNull(calibrator.getEstimatedMzy());
20339 assertNull(calibrator.getEstimatedCovariance());
20340 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20341 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20342 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20343 assertNotNull(calibrator.getGroundTruthGravityNorm());
20344 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20345 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20346 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20347 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20348 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20349 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20350 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20351 }
20352
20353 @Test
20354 public void testConstructor160() throws WrongSizeException {
20355 final Collection<StandardDeviationBodyKinematics> measurements =
20356 Collections.emptyList();
20357
20358 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20359 final double latitude = Math.toRadians(
20360 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20361 final double longitude = Math.toRadians(
20362 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20363 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20364 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20365 final NEDVelocity nedVelocity = new NEDVelocity();
20366 final ECEFPosition ecefPosition = new ECEFPosition();
20367 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20368 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20369 ecefPosition, ecefVelocity);
20370
20371 final KnownPositionAccelerometerCalibrator calibrator =
20372 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20373 this);
20374
20375
20376 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20377 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20378 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20379 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20380 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20381 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20382 final Acceleration bx2 = new Acceleration(0.0,
20383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20384 calibrator.getInitialBiasXAsAcceleration(bx2);
20385 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20386 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20387 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20388 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20389 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20390 final Acceleration by2 = new Acceleration(0.0,
20391 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20392 calibrator.getInitialBiasYAsAcceleration(by2);
20393 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20394 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20395 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20396 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20397 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20398 final Acceleration bz2 = new Acceleration(0.0,
20399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20400 calibrator.getInitialBiasZAsAcceleration(bz2);
20401 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20402 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20403 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20404 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20405 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20406 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20407 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20408 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20409 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20410 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20411 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20412 final double[] bias1 = calibrator.getInitialBias();
20413 assertArrayEquals(bias1, new double[3], 0.0);
20414 final double[] bias2 = new double[3];
20415 calibrator.getInitialBias(bias2);
20416 assertArrayEquals(bias1, bias2, 0.0);
20417 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20418 assertEquals(b1, new Matrix(3, 1));
20419 final Matrix b2 = new Matrix(3, 1);
20420 calibrator.getInitialBiasAsMatrix(b2);
20421 assertEquals(b1, b2);
20422 final Matrix ma1 = calibrator.getInitialMa();
20423 assertEquals(ma1, new Matrix(3, 3));
20424 final Matrix ma2 = new Matrix(3, 3);
20425 calibrator.getInitialMa(ma2);
20426 assertEquals(ma1, ma2);
20427 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20428 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20429 final NEDPosition nedPosition1 = new NEDPosition();
20430 assertTrue(calibrator.getNedPosition(nedPosition1));
20431 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20432 assertSame(calibrator.getMeasurements(), measurements);
20433 assertFalse(calibrator.isCommonAxisUsed());
20434 assertSame(calibrator.getListener(), this);
20435 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20436 assertFalse(calibrator.isReady());
20437 assertFalse(calibrator.isRunning());
20438 assertNull(calibrator.getEstimatedBiases());
20439 assertFalse(calibrator.getEstimatedBiases(null));
20440 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20441 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20442 assertNull(calibrator.getEstimatedBiasFx());
20443 assertNull(calibrator.getEstimatedBiasFy());
20444 assertNull(calibrator.getEstimatedBiasFz());
20445 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20446 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20447 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20448 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20449 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20450 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20451 assertNull(calibrator.getEstimatedMa());
20452 assertNull(calibrator.getEstimatedSx());
20453 assertNull(calibrator.getEstimatedSy());
20454 assertNull(calibrator.getEstimatedSz());
20455 assertNull(calibrator.getEstimatedMxy());
20456 assertNull(calibrator.getEstimatedMxz());
20457 assertNull(calibrator.getEstimatedMyx());
20458 assertNull(calibrator.getEstimatedMyz());
20459 assertNull(calibrator.getEstimatedMzx());
20460 assertNull(calibrator.getEstimatedMzy());
20461 assertNull(calibrator.getEstimatedCovariance());
20462 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20463 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20464 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20465 assertNotNull(calibrator.getGroundTruthGravityNorm());
20466 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20467 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20468 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20469 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20470 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20471 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20472 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20473 }
20474
20475 @Test
20476 public void testConstructor161() throws WrongSizeException {
20477 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20478 final double latitude = Math.toRadians(
20479 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20480 final double longitude = Math.toRadians(
20481 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20482 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20483 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20484 final NEDVelocity nedVelocity = new NEDVelocity();
20485 final ECEFPosition ecefPosition = new ECEFPosition();
20486 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20487 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20488 ecefPosition, ecefVelocity);
20489
20490 final KnownPositionAccelerometerCalibrator calibrator =
20491 new KnownPositionAccelerometerCalibrator(nedPosition,
20492 true);
20493
20494
20495 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20496 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20497 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20498 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20499 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20500 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20501 final Acceleration bx2 = new Acceleration(0.0,
20502 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20503 calibrator.getInitialBiasXAsAcceleration(bx2);
20504 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20505 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20506 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20507 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20508 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20509 final Acceleration by2 = new Acceleration(0.0,
20510 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20511 calibrator.getInitialBiasYAsAcceleration(by2);
20512 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20513 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20514 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20515 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20516 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20517 final Acceleration bz2 = new Acceleration(0.0,
20518 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20519 calibrator.getInitialBiasZAsAcceleration(bz2);
20520 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20521 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20522 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20523 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20524 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20525 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20526 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20527 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20528 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20529 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20530 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20531 final double[] bias1 = calibrator.getInitialBias();
20532 assertArrayEquals(bias1, new double[3], 0.0);
20533 final double[] bias2 = new double[3];
20534 calibrator.getInitialBias(bias2);
20535 assertArrayEquals(bias1, bias2, 0.0);
20536 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20537 assertEquals(b1, new Matrix(3, 1));
20538 final Matrix b2 = new Matrix(3, 1);
20539 calibrator.getInitialBiasAsMatrix(b2);
20540 assertEquals(b1, b2);
20541 final Matrix ma1 = calibrator.getInitialMa();
20542 assertEquals(ma1, new Matrix(3, 3));
20543 final Matrix ma2 = new Matrix(3, 3);
20544 calibrator.getInitialMa(ma2);
20545 assertEquals(ma1, ma2);
20546 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20547 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20548 final NEDPosition nedPosition1 = new NEDPosition();
20549 assertTrue(calibrator.getNedPosition(nedPosition1));
20550 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20551 assertNull(calibrator.getMeasurements());
20552 assertTrue(calibrator.isCommonAxisUsed());
20553 assertNull(calibrator.getListener());
20554 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20555 assertFalse(calibrator.isReady());
20556 assertFalse(calibrator.isRunning());
20557 assertNull(calibrator.getEstimatedBiases());
20558 assertFalse(calibrator.getEstimatedBiases(null));
20559 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20560 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20561 assertNull(calibrator.getEstimatedBiasFx());
20562 assertNull(calibrator.getEstimatedBiasFy());
20563 assertNull(calibrator.getEstimatedBiasFz());
20564 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20565 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20566 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20567 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20568 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20569 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20570 assertNull(calibrator.getEstimatedMa());
20571 assertNull(calibrator.getEstimatedSx());
20572 assertNull(calibrator.getEstimatedSy());
20573 assertNull(calibrator.getEstimatedSz());
20574 assertNull(calibrator.getEstimatedMxy());
20575 assertNull(calibrator.getEstimatedMxz());
20576 assertNull(calibrator.getEstimatedMyx());
20577 assertNull(calibrator.getEstimatedMyz());
20578 assertNull(calibrator.getEstimatedMzx());
20579 assertNull(calibrator.getEstimatedMzy());
20580 assertNull(calibrator.getEstimatedCovariance());
20581 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20582 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20583 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20584 assertNotNull(calibrator.getGroundTruthGravityNorm());
20585 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20586 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20587 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20588 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20589 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20590 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20591 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20592 }
20593
20594 @Test
20595 public void testConstructor162() throws WrongSizeException {
20596 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20597 final double latitude = Math.toRadians(
20598 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20599 final double longitude = Math.toRadians(
20600 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20601 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20602 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20603 final NEDVelocity nedVelocity = new NEDVelocity();
20604 final ECEFPosition ecefPosition = new ECEFPosition();
20605 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20606 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20607 ecefPosition, ecefVelocity);
20608
20609 final KnownPositionAccelerometerCalibrator calibrator =
20610 new KnownPositionAccelerometerCalibrator(nedPosition,
20611 true, this);
20612
20613
20614 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20615 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20616 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20617 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20618 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20619 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20620 final Acceleration bx2 = new Acceleration(0.0,
20621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20622 calibrator.getInitialBiasXAsAcceleration(bx2);
20623 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20624 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20625 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20626 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20627 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20628 final Acceleration by2 = new Acceleration(0.0,
20629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20630 calibrator.getInitialBiasYAsAcceleration(by2);
20631 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20632 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20633 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20634 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20635 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20636 final Acceleration bz2 = new Acceleration(0.0,
20637 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20638 calibrator.getInitialBiasZAsAcceleration(bz2);
20639 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20640 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20641 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20642 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20643 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20644 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20645 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20646 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20647 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20648 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20649 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20650 final double[] bias1 = calibrator.getInitialBias();
20651 assertArrayEquals(bias1, new double[3], 0.0);
20652 final double[] bias2 = new double[3];
20653 calibrator.getInitialBias(bias2);
20654 assertArrayEquals(bias1, bias2, 0.0);
20655 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20656 assertEquals(b1, new Matrix(3, 1));
20657 final Matrix b2 = new Matrix(3, 1);
20658 calibrator.getInitialBiasAsMatrix(b2);
20659 assertEquals(b1, b2);
20660 final Matrix ma1 = calibrator.getInitialMa();
20661 assertEquals(ma1, new Matrix(3, 3));
20662 final Matrix ma2 = new Matrix(3, 3);
20663 calibrator.getInitialMa(ma2);
20664 assertEquals(ma1, ma2);
20665 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20666 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20667 final NEDPosition nedPosition1 = new NEDPosition();
20668 assertTrue(calibrator.getNedPosition(nedPosition1));
20669 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20670 assertNull(calibrator.getMeasurements());
20671 assertTrue(calibrator.isCommonAxisUsed());
20672 assertSame(calibrator.getListener(), this);
20673 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20674 assertFalse(calibrator.isReady());
20675 assertFalse(calibrator.isRunning());
20676 assertNull(calibrator.getEstimatedBiases());
20677 assertFalse(calibrator.getEstimatedBiases(null));
20678 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20679 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20680 assertNull(calibrator.getEstimatedBiasFx());
20681 assertNull(calibrator.getEstimatedBiasFy());
20682 assertNull(calibrator.getEstimatedBiasFz());
20683 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20684 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20685 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20686 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20687 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20688 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20689 assertNull(calibrator.getEstimatedMa());
20690 assertNull(calibrator.getEstimatedSx());
20691 assertNull(calibrator.getEstimatedSy());
20692 assertNull(calibrator.getEstimatedSz());
20693 assertNull(calibrator.getEstimatedMxy());
20694 assertNull(calibrator.getEstimatedMxz());
20695 assertNull(calibrator.getEstimatedMyx());
20696 assertNull(calibrator.getEstimatedMyz());
20697 assertNull(calibrator.getEstimatedMzx());
20698 assertNull(calibrator.getEstimatedMzy());
20699 assertNull(calibrator.getEstimatedCovariance());
20700 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20701 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20702 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20703 assertNotNull(calibrator.getGroundTruthGravityNorm());
20704 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20705 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20706 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20707 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20708 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20709 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20710 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20711 }
20712
20713 @Test
20714 public void testConstructor163() throws WrongSizeException {
20715 final Collection<StandardDeviationBodyKinematics> measurements =
20716 Collections.emptyList();
20717 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20718 final double latitude = Math.toRadians(
20719 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20720 final double longitude = Math.toRadians(
20721 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20722 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20723 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20724 final NEDVelocity nedVelocity = new NEDVelocity();
20725 final ECEFPosition ecefPosition = new ECEFPosition();
20726 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20727 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20728 ecefPosition, ecefVelocity);
20729
20730 final KnownPositionAccelerometerCalibrator calibrator =
20731 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20732 true);
20733
20734
20735 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20736 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20737 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20738 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20739 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20740 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20741 final Acceleration bx2 = new Acceleration(0.0,
20742 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20743 calibrator.getInitialBiasXAsAcceleration(bx2);
20744 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20745 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20746 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20747 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20748 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20749 final Acceleration by2 = new Acceleration(0.0,
20750 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20751 calibrator.getInitialBiasYAsAcceleration(by2);
20752 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20753 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20754 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20755 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20756 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20757 final Acceleration bz2 = new Acceleration(0.0,
20758 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20759 calibrator.getInitialBiasZAsAcceleration(bz2);
20760 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20761 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20762 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20763 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20764 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20765 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20766 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20767 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20768 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20769 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20770 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20771 final double[] bias1 = calibrator.getInitialBias();
20772 assertArrayEquals(bias1, new double[3], 0.0);
20773 final double[] bias2 = new double[3];
20774 calibrator.getInitialBias(bias2);
20775 assertArrayEquals(bias1, bias2, 0.0);
20776 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20777 assertEquals(b1, new Matrix(3, 1));
20778 final Matrix b2 = new Matrix(3, 1);
20779 calibrator.getInitialBiasAsMatrix(b2);
20780 assertEquals(b1, b2);
20781 final Matrix ma1 = calibrator.getInitialMa();
20782 assertEquals(ma1, new Matrix(3, 3));
20783 final Matrix ma2 = new Matrix(3, 3);
20784 calibrator.getInitialMa(ma2);
20785 assertEquals(ma1, ma2);
20786 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20787 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20788 final NEDPosition nedPosition1 = new NEDPosition();
20789 assertTrue(calibrator.getNedPosition(nedPosition1));
20790 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20791 assertSame(calibrator.getMeasurements(), measurements);
20792 assertTrue(calibrator.isCommonAxisUsed());
20793 assertNull(calibrator.getListener());
20794 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20795 assertFalse(calibrator.isReady());
20796 assertFalse(calibrator.isRunning());
20797 assertNull(calibrator.getEstimatedBiases());
20798 assertFalse(calibrator.getEstimatedBiases(null));
20799 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20800 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20801 assertNull(calibrator.getEstimatedBiasFx());
20802 assertNull(calibrator.getEstimatedBiasFy());
20803 assertNull(calibrator.getEstimatedBiasFz());
20804 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20805 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20806 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20807 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20808 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20809 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20810 assertNull(calibrator.getEstimatedMa());
20811 assertNull(calibrator.getEstimatedSx());
20812 assertNull(calibrator.getEstimatedSy());
20813 assertNull(calibrator.getEstimatedSz());
20814 assertNull(calibrator.getEstimatedMxy());
20815 assertNull(calibrator.getEstimatedMxz());
20816 assertNull(calibrator.getEstimatedMyx());
20817 assertNull(calibrator.getEstimatedMyz());
20818 assertNull(calibrator.getEstimatedMzx());
20819 assertNull(calibrator.getEstimatedMzy());
20820 assertNull(calibrator.getEstimatedCovariance());
20821 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20822 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20823 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20824 assertNotNull(calibrator.getGroundTruthGravityNorm());
20825 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20826 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20827 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20828 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20829 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20830 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20831 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20832 }
20833
20834 @Test
20835 public void testConstructor164() throws WrongSizeException {
20836 final Collection<StandardDeviationBodyKinematics> measurements =
20837 Collections.emptyList();
20838 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20839 final double latitude = Math.toRadians(
20840 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20841 final double longitude = Math.toRadians(
20842 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20843 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20844 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20845 final NEDVelocity nedVelocity = new NEDVelocity();
20846 final ECEFPosition ecefPosition = new ECEFPosition();
20847 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20848 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20849 ecefPosition, ecefVelocity);
20850
20851 final KnownPositionAccelerometerCalibrator calibrator =
20852 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20853 true, this);
20854
20855
20856 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20857 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20858 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20859 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20860 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20861 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20862 final Acceleration bx2 = new Acceleration(0.0,
20863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20864 calibrator.getInitialBiasXAsAcceleration(bx2);
20865 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20866 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20867 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20868 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20869 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20870 final Acceleration by2 = new Acceleration(0.0,
20871 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20872 calibrator.getInitialBiasYAsAcceleration(by2);
20873 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20874 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20875 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20876 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20877 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20878 final Acceleration bz2 = new Acceleration(0.0,
20879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20880 calibrator.getInitialBiasZAsAcceleration(bz2);
20881 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20882 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20883 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20884 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20885 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20886 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20887 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20888 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20889 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20890 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20891 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20892 final double[] bias1 = calibrator.getInitialBias();
20893 assertArrayEquals(bias1, new double[3], 0.0);
20894 final double[] bias2 = new double[3];
20895 calibrator.getInitialBias(bias2);
20896 assertArrayEquals(bias1, bias2, 0.0);
20897 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20898 assertEquals(b1, new Matrix(3, 1));
20899 final Matrix b2 = new Matrix(3, 1);
20900 calibrator.getInitialBiasAsMatrix(b2);
20901 assertEquals(b1, b2);
20902 final Matrix ma1 = calibrator.getInitialMa();
20903 assertEquals(ma1, new Matrix(3, 3));
20904 final Matrix ma2 = new Matrix(3, 3);
20905 calibrator.getInitialMa(ma2);
20906 assertEquals(ma1, ma2);
20907 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20908 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20909 final NEDPosition nedPosition1 = new NEDPosition();
20910 assertTrue(calibrator.getNedPosition(nedPosition1));
20911 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20912 assertSame(calibrator.getMeasurements(), measurements);
20913 assertTrue(calibrator.isCommonAxisUsed());
20914 assertSame(calibrator.getListener(), this);
20915 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20916 assertFalse(calibrator.isReady());
20917 assertFalse(calibrator.isRunning());
20918 assertNull(calibrator.getEstimatedBiases());
20919 assertFalse(calibrator.getEstimatedBiases(null));
20920 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20921 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20922 assertNull(calibrator.getEstimatedBiasFx());
20923 assertNull(calibrator.getEstimatedBiasFy());
20924 assertNull(calibrator.getEstimatedBiasFz());
20925 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20926 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20927 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20928 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20929 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20930 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20931 assertNull(calibrator.getEstimatedMa());
20932 assertNull(calibrator.getEstimatedSx());
20933 assertNull(calibrator.getEstimatedSy());
20934 assertNull(calibrator.getEstimatedSz());
20935 assertNull(calibrator.getEstimatedMxy());
20936 assertNull(calibrator.getEstimatedMxz());
20937 assertNull(calibrator.getEstimatedMyx());
20938 assertNull(calibrator.getEstimatedMyz());
20939 assertNull(calibrator.getEstimatedMzx());
20940 assertNull(calibrator.getEstimatedMzy());
20941 assertNull(calibrator.getEstimatedCovariance());
20942 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20943 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20944 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20945 assertNotNull(calibrator.getGroundTruthGravityNorm());
20946 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20947 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20948 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20949 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20950 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20951 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20952 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20953 }
20954
20955 @Test
20956 public void testConstructor165() throws WrongSizeException {
20957 final Matrix ba = generateBa();
20958 final double biasX = ba.getElementAtIndex(0);
20959 final double biasY = ba.getElementAtIndex(1);
20960 final double biasZ = ba.getElementAtIndex(2);
20961
20962 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20963 final double latitude = Math.toRadians(
20964 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20965 final double longitude = Math.toRadians(
20966 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20967 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20968 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20969 final NEDVelocity nedVelocity = new NEDVelocity();
20970 final ECEFPosition ecefPosition = new ECEFPosition();
20971 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20972 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20973 ecefPosition, ecefVelocity);
20974
20975 final KnownPositionAccelerometerCalibrator calibrator =
20976 new KnownPositionAccelerometerCalibrator(nedPosition,
20977 biasX, biasY, biasZ);
20978
20979
20980 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20981 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20982 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20983 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20984 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20985 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20986 final Acceleration bx2 = new Acceleration(0.0,
20987 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20988 calibrator.getInitialBiasXAsAcceleration(bx2);
20989 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20990 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20991 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20992 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20993 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20994 final Acceleration by2 = new Acceleration(0.0,
20995 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20996 calibrator.getInitialBiasYAsAcceleration(by2);
20997 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20998 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20999 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21000 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21001 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21002 final Acceleration bz2 = new Acceleration(0.0,
21003 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21004 calibrator.getInitialBiasZAsAcceleration(bz2);
21005 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21006 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21007 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21008 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21009 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21010 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21011 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21012 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21013 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21014 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21015 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21016 final double[] bias1 = calibrator.getInitialBias();
21017 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21018 final double[] bias2 = new double[3];
21019 calibrator.getInitialBias(bias2);
21020 assertArrayEquals(bias1, bias2, 0.0);
21021 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21022 assertEquals(b1, ba);
21023 final Matrix b2 = new Matrix(3, 1);
21024 calibrator.getInitialBiasAsMatrix(b2);
21025 assertEquals(b1, b2);
21026 final Matrix ma1 = calibrator.getInitialMa();
21027 assertEquals(ma1, new Matrix(3, 3));
21028 final Matrix ma2 = new Matrix(3, 3);
21029 calibrator.getInitialMa(ma2);
21030 assertEquals(ma1, ma2);
21031 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21032 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21033 final NEDPosition nedPosition1 = new NEDPosition();
21034 assertTrue(calibrator.getNedPosition(nedPosition1));
21035 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21036 assertNull(calibrator.getMeasurements());
21037 assertFalse(calibrator.isCommonAxisUsed());
21038 assertNull(calibrator.getListener());
21039 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21040 assertFalse(calibrator.isReady());
21041 assertFalse(calibrator.isRunning());
21042 assertNull(calibrator.getEstimatedBiases());
21043 assertFalse(calibrator.getEstimatedBiases(null));
21044 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21045 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21046 assertNull(calibrator.getEstimatedBiasFx());
21047 assertNull(calibrator.getEstimatedBiasFy());
21048 assertNull(calibrator.getEstimatedBiasFz());
21049 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21050 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21051 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21052 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21053 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21054 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21055 assertNull(calibrator.getEstimatedMa());
21056 assertNull(calibrator.getEstimatedSx());
21057 assertNull(calibrator.getEstimatedSy());
21058 assertNull(calibrator.getEstimatedSz());
21059 assertNull(calibrator.getEstimatedMxy());
21060 assertNull(calibrator.getEstimatedMxz());
21061 assertNull(calibrator.getEstimatedMyx());
21062 assertNull(calibrator.getEstimatedMyz());
21063 assertNull(calibrator.getEstimatedMzx());
21064 assertNull(calibrator.getEstimatedMzy());
21065 assertNull(calibrator.getEstimatedCovariance());
21066 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21067 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21068 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21069 assertNotNull(calibrator.getGroundTruthGravityNorm());
21070 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21071 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21072 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21073 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21074 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21075 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21076 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21077 }
21078
21079 @Test
21080 public void testConstructor166() throws WrongSizeException {
21081 final Matrix ba = generateBa();
21082 final double biasX = ba.getElementAtIndex(0);
21083 final double biasY = ba.getElementAtIndex(1);
21084 final double biasZ = ba.getElementAtIndex(2);
21085
21086 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21087 final double latitude = Math.toRadians(
21088 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21089 final double longitude = Math.toRadians(
21090 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21091 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21092 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21093 final NEDVelocity nedVelocity = new NEDVelocity();
21094 final ECEFPosition ecefPosition = new ECEFPosition();
21095 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21096 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21097 ecefPosition, ecefVelocity);
21098
21099 final KnownPositionAccelerometerCalibrator calibrator =
21100 new KnownPositionAccelerometerCalibrator(nedPosition,
21101 biasX, biasY, biasZ, this);
21102
21103
21104 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21105 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21106 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21107 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21108 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21109 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21110 final Acceleration bx2 = new Acceleration(0.0,
21111 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21112 calibrator.getInitialBiasXAsAcceleration(bx2);
21113 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21114 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21115 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21116 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21117 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21118 final Acceleration by2 = new Acceleration(0.0,
21119 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21120 calibrator.getInitialBiasYAsAcceleration(by2);
21121 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21122 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21123 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21124 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21125 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21126 final Acceleration bz2 = new Acceleration(0.0,
21127 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21128 calibrator.getInitialBiasZAsAcceleration(bz2);
21129 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21130 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21131 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21132 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21133 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21134 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21135 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21136 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21137 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21138 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21139 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21140 final double[] bias1 = calibrator.getInitialBias();
21141 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21142 final double[] bias2 = new double[3];
21143 calibrator.getInitialBias(bias2);
21144 assertArrayEquals(bias1, bias2, 0.0);
21145 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21146 assertEquals(b1, ba);
21147 final Matrix b2 = new Matrix(3, 1);
21148 calibrator.getInitialBiasAsMatrix(b2);
21149 assertEquals(b1, b2);
21150 final Matrix ma1 = calibrator.getInitialMa();
21151 assertEquals(ma1, new Matrix(3, 3));
21152 final Matrix ma2 = new Matrix(3, 3);
21153 calibrator.getInitialMa(ma2);
21154 assertEquals(ma1, ma2);
21155 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21156 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21157 final NEDPosition nedPosition1 = new NEDPosition();
21158 assertTrue(calibrator.getNedPosition(nedPosition1));
21159 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21160 assertNull(calibrator.getMeasurements());
21161 assertFalse(calibrator.isCommonAxisUsed());
21162 assertSame(calibrator.getListener(), this);
21163 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21164 assertFalse(calibrator.isReady());
21165 assertFalse(calibrator.isRunning());
21166 assertNull(calibrator.getEstimatedBiases());
21167 assertFalse(calibrator.getEstimatedBiases(null));
21168 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21169 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21170 assertNull(calibrator.getEstimatedBiasFx());
21171 assertNull(calibrator.getEstimatedBiasFy());
21172 assertNull(calibrator.getEstimatedBiasFz());
21173 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21174 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21175 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21176 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21177 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21178 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21179 assertNull(calibrator.getEstimatedMa());
21180 assertNull(calibrator.getEstimatedSx());
21181 assertNull(calibrator.getEstimatedSy());
21182 assertNull(calibrator.getEstimatedSz());
21183 assertNull(calibrator.getEstimatedMxy());
21184 assertNull(calibrator.getEstimatedMxz());
21185 assertNull(calibrator.getEstimatedMyx());
21186 assertNull(calibrator.getEstimatedMyz());
21187 assertNull(calibrator.getEstimatedMzx());
21188 assertNull(calibrator.getEstimatedMzy());
21189 assertNull(calibrator.getEstimatedCovariance());
21190 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21191 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21192 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21193 assertNotNull(calibrator.getGroundTruthGravityNorm());
21194 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21195 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21196 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21197 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21198 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21199 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21200 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21201 }
21202
21203 @Test
21204 public void testConstructor167() throws WrongSizeException {
21205 final Collection<StandardDeviationBodyKinematics> measurements =
21206 Collections.emptyList();
21207
21208 final Matrix ba = generateBa();
21209 final double biasX = ba.getElementAtIndex(0);
21210 final double biasY = ba.getElementAtIndex(1);
21211 final double biasZ = ba.getElementAtIndex(2);
21212
21213 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21214 final double latitude = Math.toRadians(
21215 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21216 final double longitude = Math.toRadians(
21217 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21218 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21219 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21220 final NEDVelocity nedVelocity = new NEDVelocity();
21221 final ECEFPosition ecefPosition = new ECEFPosition();
21222 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21223 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21224 ecefPosition, ecefVelocity);
21225
21226 final KnownPositionAccelerometerCalibrator calibrator =
21227 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21228 biasX, biasY, biasZ);
21229
21230
21231 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21232 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21233 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21234 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21235 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21236 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21237 final Acceleration bx2 = new Acceleration(0.0,
21238 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21239 calibrator.getInitialBiasXAsAcceleration(bx2);
21240 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21241 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21242 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21243 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21244 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21245 final Acceleration by2 = new Acceleration(0.0,
21246 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21247 calibrator.getInitialBiasYAsAcceleration(by2);
21248 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21249 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21250 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21251 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21252 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21253 final Acceleration bz2 = new Acceleration(0.0,
21254 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21255 calibrator.getInitialBiasZAsAcceleration(bz2);
21256 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21257 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21258 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21259 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21260 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21261 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21262 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21263 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21264 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21265 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21266 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21267 final double[] bias1 = calibrator.getInitialBias();
21268 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21269 final double[] bias2 = new double[3];
21270 calibrator.getInitialBias(bias2);
21271 assertArrayEquals(bias1, bias2, 0.0);
21272 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21273 assertEquals(b1, ba);
21274 final Matrix b2 = new Matrix(3, 1);
21275 calibrator.getInitialBiasAsMatrix(b2);
21276 assertEquals(b1, b2);
21277 final Matrix ma1 = calibrator.getInitialMa();
21278 assertEquals(ma1, new Matrix(3, 3));
21279 final Matrix ma2 = new Matrix(3, 3);
21280 calibrator.getInitialMa(ma2);
21281 assertEquals(ma1, ma2);
21282 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21283 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21284 final NEDPosition nedPosition1 = new NEDPosition();
21285 assertTrue(calibrator.getNedPosition(nedPosition1));
21286 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21287 assertSame(calibrator.getMeasurements(), measurements);
21288 assertFalse(calibrator.isCommonAxisUsed());
21289 assertNull(calibrator.getListener());
21290 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21291 assertFalse(calibrator.isReady());
21292 assertFalse(calibrator.isRunning());
21293 assertNull(calibrator.getEstimatedBiases());
21294 assertFalse(calibrator.getEstimatedBiases(null));
21295 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21296 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21297 assertNull(calibrator.getEstimatedBiasFx());
21298 assertNull(calibrator.getEstimatedBiasFy());
21299 assertNull(calibrator.getEstimatedBiasFz());
21300 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21301 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21302 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21303 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21304 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21305 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21306 assertNull(calibrator.getEstimatedMa());
21307 assertNull(calibrator.getEstimatedSx());
21308 assertNull(calibrator.getEstimatedSy());
21309 assertNull(calibrator.getEstimatedSz());
21310 assertNull(calibrator.getEstimatedMxy());
21311 assertNull(calibrator.getEstimatedMxz());
21312 assertNull(calibrator.getEstimatedMyx());
21313 assertNull(calibrator.getEstimatedMyz());
21314 assertNull(calibrator.getEstimatedMzx());
21315 assertNull(calibrator.getEstimatedMzy());
21316 assertNull(calibrator.getEstimatedCovariance());
21317 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21318 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21319 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21320 assertNotNull(calibrator.getGroundTruthGravityNorm());
21321 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21322 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21323 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21324 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21325 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21326 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21327 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21328 }
21329
21330 @Test
21331 public void testConstructor168() throws WrongSizeException {
21332 final Collection<StandardDeviationBodyKinematics> measurements =
21333 Collections.emptyList();
21334
21335 final Matrix ba = generateBa();
21336 final double biasX = ba.getElementAtIndex(0);
21337 final double biasY = ba.getElementAtIndex(1);
21338 final double biasZ = ba.getElementAtIndex(2);
21339
21340 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21341 final double latitude = Math.toRadians(
21342 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21343 final double longitude = Math.toRadians(
21344 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21345 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21346 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21347 final NEDVelocity nedVelocity = new NEDVelocity();
21348 final ECEFPosition ecefPosition = new ECEFPosition();
21349 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21350 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21351 ecefPosition, ecefVelocity);
21352
21353 final KnownPositionAccelerometerCalibrator calibrator =
21354 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21355 biasX, biasY, biasZ, this);
21356
21357
21358 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21359 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21360 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21361 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21362 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21363 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21364 final Acceleration bx2 = new Acceleration(0.0,
21365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21366 calibrator.getInitialBiasXAsAcceleration(bx2);
21367 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21368 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21369 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21370 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21371 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21372 final Acceleration by2 = new Acceleration(0.0,
21373 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21374 calibrator.getInitialBiasYAsAcceleration(by2);
21375 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21376 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21377 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21378 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21379 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21380 final Acceleration bz2 = new Acceleration(0.0,
21381 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21382 calibrator.getInitialBiasZAsAcceleration(bz2);
21383 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21384 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21385 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21386 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21387 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21388 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21389 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21390 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21391 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21392 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21393 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21394 final double[] bias1 = calibrator.getInitialBias();
21395 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21396 final double[] bias2 = new double[3];
21397 calibrator.getInitialBias(bias2);
21398 assertArrayEquals(bias1, bias2, 0.0);
21399 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21400 assertEquals(b1, ba);
21401 final Matrix b2 = new Matrix(3, 1);
21402 calibrator.getInitialBiasAsMatrix(b2);
21403 assertEquals(b1, b2);
21404 final Matrix ma1 = calibrator.getInitialMa();
21405 assertEquals(ma1, new Matrix(3, 3));
21406 final Matrix ma2 = new Matrix(3, 3);
21407 calibrator.getInitialMa(ma2);
21408 assertEquals(ma1, ma2);
21409 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21410 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21411 final NEDPosition nedPosition1 = new NEDPosition();
21412 assertTrue(calibrator.getNedPosition(nedPosition1));
21413 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21414 assertSame(calibrator.getMeasurements(), measurements);
21415 assertFalse(calibrator.isCommonAxisUsed());
21416 assertSame(calibrator.getListener(), this);
21417 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21418 assertFalse(calibrator.isReady());
21419 assertFalse(calibrator.isRunning());
21420 assertNull(calibrator.getEstimatedBiases());
21421 assertFalse(calibrator.getEstimatedBiases(null));
21422 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21423 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21424 assertNull(calibrator.getEstimatedBiasFx());
21425 assertNull(calibrator.getEstimatedBiasFy());
21426 assertNull(calibrator.getEstimatedBiasFz());
21427 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21428 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21429 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21430 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21431 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21432 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21433 assertNull(calibrator.getEstimatedMa());
21434 assertNull(calibrator.getEstimatedSx());
21435 assertNull(calibrator.getEstimatedSy());
21436 assertNull(calibrator.getEstimatedSz());
21437 assertNull(calibrator.getEstimatedMxy());
21438 assertNull(calibrator.getEstimatedMxz());
21439 assertNull(calibrator.getEstimatedMyx());
21440 assertNull(calibrator.getEstimatedMyz());
21441 assertNull(calibrator.getEstimatedMzx());
21442 assertNull(calibrator.getEstimatedMzy());
21443 assertNull(calibrator.getEstimatedCovariance());
21444 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21445 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21446 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21447 assertNotNull(calibrator.getGroundTruthGravityNorm());
21448 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21449 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21450 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21451 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21452 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21453 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21454 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21455 }
21456
21457 @Test
21458 public void testConstructor169() throws WrongSizeException {
21459 final Matrix ba = generateBa();
21460 final double biasX = ba.getElementAtIndex(0);
21461 final double biasY = ba.getElementAtIndex(1);
21462 final double biasZ = ba.getElementAtIndex(2);
21463
21464 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21465 final double latitude = Math.toRadians(
21466 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21467 final double longitude = Math.toRadians(
21468 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21469 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21470 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21471 final NEDVelocity nedVelocity = new NEDVelocity();
21472 final ECEFPosition ecefPosition = new ECEFPosition();
21473 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21474 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21475 ecefPosition, ecefVelocity);
21476
21477 final KnownPositionAccelerometerCalibrator calibrator =
21478 new KnownPositionAccelerometerCalibrator(nedPosition,
21479 true, biasX, biasY, biasZ);
21480
21481
21482 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21483 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21484 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21485 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21486 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21487 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21488 final Acceleration bx2 = new Acceleration(0.0,
21489 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21490 calibrator.getInitialBiasXAsAcceleration(bx2);
21491 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21492 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21493 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21494 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21495 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21496 final Acceleration by2 = new Acceleration(0.0,
21497 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21498 calibrator.getInitialBiasYAsAcceleration(by2);
21499 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21500 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21501 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21502 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21503 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21504 final Acceleration bz2 = new Acceleration(0.0,
21505 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21506 calibrator.getInitialBiasZAsAcceleration(bz2);
21507 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21508 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21509 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21510 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21511 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21512 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21513 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21514 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21515 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21516 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21517 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21518 final double[] bias1 = calibrator.getInitialBias();
21519 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21520 final double[] bias2 = new double[3];
21521 calibrator.getInitialBias(bias2);
21522 assertArrayEquals(bias1, bias2, 0.0);
21523 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21524 assertEquals(b1, ba);
21525 final Matrix b2 = new Matrix(3, 1);
21526 calibrator.getInitialBiasAsMatrix(b2);
21527 assertEquals(b1, b2);
21528 final Matrix ma1 = calibrator.getInitialMa();
21529 assertEquals(ma1, new Matrix(3, 3));
21530 final Matrix ma2 = new Matrix(3, 3);
21531 calibrator.getInitialMa(ma2);
21532 assertEquals(ma1, ma2);
21533 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21534 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21535 final NEDPosition nedPosition1 = new NEDPosition();
21536 assertTrue(calibrator.getNedPosition(nedPosition1));
21537 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21538 assertNull(calibrator.getMeasurements());
21539 assertTrue(calibrator.isCommonAxisUsed());
21540 assertNull(calibrator.getListener());
21541 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21542 assertFalse(calibrator.isReady());
21543 assertFalse(calibrator.isRunning());
21544 assertNull(calibrator.getEstimatedBiases());
21545 assertFalse(calibrator.getEstimatedBiases(null));
21546 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21547 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21548 assertNull(calibrator.getEstimatedBiasFx());
21549 assertNull(calibrator.getEstimatedBiasFy());
21550 assertNull(calibrator.getEstimatedBiasFz());
21551 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21552 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21553 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21554 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21555 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21556 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21557 assertNull(calibrator.getEstimatedMa());
21558 assertNull(calibrator.getEstimatedSx());
21559 assertNull(calibrator.getEstimatedSy());
21560 assertNull(calibrator.getEstimatedSz());
21561 assertNull(calibrator.getEstimatedMxy());
21562 assertNull(calibrator.getEstimatedMxz());
21563 assertNull(calibrator.getEstimatedMyx());
21564 assertNull(calibrator.getEstimatedMyz());
21565 assertNull(calibrator.getEstimatedMzx());
21566 assertNull(calibrator.getEstimatedMzy());
21567 assertNull(calibrator.getEstimatedCovariance());
21568 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21569 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21570 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21571 assertNotNull(calibrator.getGroundTruthGravityNorm());
21572 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21573 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21574 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21575 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21576 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21577 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21578 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21579 }
21580
21581 @Test
21582 public void testConstructor170() throws WrongSizeException {
21583 final Matrix ba = generateBa();
21584 final double biasX = ba.getElementAtIndex(0);
21585 final double biasY = ba.getElementAtIndex(1);
21586 final double biasZ = ba.getElementAtIndex(2);
21587
21588 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21589 final double latitude = Math.toRadians(
21590 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21591 final double longitude = Math.toRadians(
21592 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21593 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21594 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21595 final NEDVelocity nedVelocity = new NEDVelocity();
21596 final ECEFPosition ecefPosition = new ECEFPosition();
21597 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21598 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21599 ecefPosition, ecefVelocity);
21600
21601 final KnownPositionAccelerometerCalibrator calibrator =
21602 new KnownPositionAccelerometerCalibrator(nedPosition,
21603 true, biasX, biasY, biasZ, this);
21604
21605
21606 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21607 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21608 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21609 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21610 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21611 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21612 final Acceleration bx2 = new Acceleration(0.0,
21613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21614 calibrator.getInitialBiasXAsAcceleration(bx2);
21615 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21616 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21617 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21618 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21619 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21620 final Acceleration by2 = new Acceleration(0.0,
21621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21622 calibrator.getInitialBiasYAsAcceleration(by2);
21623 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21624 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21625 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21626 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21627 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21628 final Acceleration bz2 = new Acceleration(0.0,
21629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21630 calibrator.getInitialBiasZAsAcceleration(bz2);
21631 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21632 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21633 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21634 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21635 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21636 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21637 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21638 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21639 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21640 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21641 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21642 final double[] bias1 = calibrator.getInitialBias();
21643 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21644 final double[] bias2 = new double[3];
21645 calibrator.getInitialBias(bias2);
21646 assertArrayEquals(bias1, bias2, 0.0);
21647 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21648 assertEquals(b1, ba);
21649 final Matrix b2 = new Matrix(3, 1);
21650 calibrator.getInitialBiasAsMatrix(b2);
21651 assertEquals(b1, b2);
21652 final Matrix ma1 = calibrator.getInitialMa();
21653 assertEquals(ma1, new Matrix(3, 3));
21654 final Matrix ma2 = new Matrix(3, 3);
21655 calibrator.getInitialMa(ma2);
21656 assertEquals(ma1, ma2);
21657 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21658 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21659 final NEDPosition nedPosition1 = new NEDPosition();
21660 assertTrue(calibrator.getNedPosition(nedPosition1));
21661 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21662 assertNull(calibrator.getMeasurements());
21663 assertTrue(calibrator.isCommonAxisUsed());
21664 assertSame(calibrator.getListener(), this);
21665 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21666 assertFalse(calibrator.isReady());
21667 assertFalse(calibrator.isRunning());
21668 assertNull(calibrator.getEstimatedBiases());
21669 assertFalse(calibrator.getEstimatedBiases(null));
21670 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21671 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21672 assertNull(calibrator.getEstimatedBiasFx());
21673 assertNull(calibrator.getEstimatedBiasFy());
21674 assertNull(calibrator.getEstimatedBiasFz());
21675 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21676 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21677 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21678 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21679 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21680 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21681 assertNull(calibrator.getEstimatedMa());
21682 assertNull(calibrator.getEstimatedSx());
21683 assertNull(calibrator.getEstimatedSy());
21684 assertNull(calibrator.getEstimatedSz());
21685 assertNull(calibrator.getEstimatedMxy());
21686 assertNull(calibrator.getEstimatedMxz());
21687 assertNull(calibrator.getEstimatedMyx());
21688 assertNull(calibrator.getEstimatedMyz());
21689 assertNull(calibrator.getEstimatedMzx());
21690 assertNull(calibrator.getEstimatedMzy());
21691 assertNull(calibrator.getEstimatedCovariance());
21692 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21693 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21694 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21695 assertNotNull(calibrator.getGroundTruthGravityNorm());
21696 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21697 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21698 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21699 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21700 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21701 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21702 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21703 }
21704
21705 @Test
21706 public void testConstructor171() throws WrongSizeException {
21707 final Collection<StandardDeviationBodyKinematics> measurements =
21708 Collections.emptyList();
21709
21710 final Matrix ba = generateBa();
21711 final double biasX = ba.getElementAtIndex(0);
21712 final double biasY = ba.getElementAtIndex(1);
21713 final double biasZ = ba.getElementAtIndex(2);
21714
21715 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21716 final double latitude = Math.toRadians(
21717 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21718 final double longitude = Math.toRadians(
21719 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21720 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21721 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21722 final NEDVelocity nedVelocity = new NEDVelocity();
21723 final ECEFPosition ecefPosition = new ECEFPosition();
21724 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21725 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21726 ecefPosition, ecefVelocity);
21727
21728 final KnownPositionAccelerometerCalibrator calibrator =
21729 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21730 true, biasX, biasY, biasZ);
21731
21732
21733 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21734 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21735 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21736 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21737 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21738 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21739 final Acceleration bx2 = new Acceleration(0.0,
21740 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21741 calibrator.getInitialBiasXAsAcceleration(bx2);
21742 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21743 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21744 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21745 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21746 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21747 final Acceleration by2 = new Acceleration(0.0,
21748 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21749 calibrator.getInitialBiasYAsAcceleration(by2);
21750 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21751 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21752 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21753 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21754 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21755 final Acceleration bz2 = new Acceleration(0.0,
21756 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21757 calibrator.getInitialBiasZAsAcceleration(bz2);
21758 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21759 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21760 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21761 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21762 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21763 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21764 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21765 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21766 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21767 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21768 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21769 final double[] bias1 = calibrator.getInitialBias();
21770 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21771 final double[] bias2 = new double[3];
21772 calibrator.getInitialBias(bias2);
21773 assertArrayEquals(bias1, bias2, 0.0);
21774 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21775 assertEquals(b1, ba);
21776 final Matrix b2 = new Matrix(3, 1);
21777 calibrator.getInitialBiasAsMatrix(b2);
21778 assertEquals(b1, b2);
21779 final Matrix ma1 = calibrator.getInitialMa();
21780 assertEquals(ma1, new Matrix(3, 3));
21781 final Matrix ma2 = new Matrix(3, 3);
21782 calibrator.getInitialMa(ma2);
21783 assertEquals(ma1, ma2);
21784 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21785 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21786 final NEDPosition nedPosition1 = new NEDPosition();
21787 assertTrue(calibrator.getNedPosition(nedPosition1));
21788 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21789 assertSame(calibrator.getMeasurements(), measurements);
21790 assertTrue(calibrator.isCommonAxisUsed());
21791 assertNull(calibrator.getListener());
21792 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21793 assertFalse(calibrator.isReady());
21794 assertFalse(calibrator.isRunning());
21795 assertNull(calibrator.getEstimatedBiases());
21796 assertFalse(calibrator.getEstimatedBiases(null));
21797 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21798 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21799 assertNull(calibrator.getEstimatedBiasFx());
21800 assertNull(calibrator.getEstimatedBiasFy());
21801 assertNull(calibrator.getEstimatedBiasFz());
21802 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21803 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21804 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21805 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21806 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21807 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21808 assertNull(calibrator.getEstimatedMa());
21809 assertNull(calibrator.getEstimatedSx());
21810 assertNull(calibrator.getEstimatedSy());
21811 assertNull(calibrator.getEstimatedSz());
21812 assertNull(calibrator.getEstimatedMxy());
21813 assertNull(calibrator.getEstimatedMxz());
21814 assertNull(calibrator.getEstimatedMyx());
21815 assertNull(calibrator.getEstimatedMyz());
21816 assertNull(calibrator.getEstimatedMzx());
21817 assertNull(calibrator.getEstimatedMzy());
21818 assertNull(calibrator.getEstimatedCovariance());
21819 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21820 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21821 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21822 assertNotNull(calibrator.getGroundTruthGravityNorm());
21823 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21824 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21825 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21826 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21827 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21828 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21829 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21830 }
21831
21832 @Test
21833 public void testConstructor172() throws WrongSizeException {
21834 final Collection<StandardDeviationBodyKinematics> measurements =
21835 Collections.emptyList();
21836
21837 final Matrix ba = generateBa();
21838 final double biasX = ba.getElementAtIndex(0);
21839 final double biasY = ba.getElementAtIndex(1);
21840 final double biasZ = ba.getElementAtIndex(2);
21841
21842 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21843 final double latitude = Math.toRadians(
21844 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21845 final double longitude = Math.toRadians(
21846 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21847 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21848 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21849 final NEDVelocity nedVelocity = new NEDVelocity();
21850 final ECEFPosition ecefPosition = new ECEFPosition();
21851 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21852 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21853 ecefPosition, ecefVelocity);
21854
21855 final KnownPositionAccelerometerCalibrator calibrator =
21856 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21857 true, biasX, biasY, biasZ, this);
21858
21859
21860 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21861 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21862 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21863 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21864 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21865 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21866 final Acceleration bx2 = new Acceleration(0.0,
21867 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21868 calibrator.getInitialBiasXAsAcceleration(bx2);
21869 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21870 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21871 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21872 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21873 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21874 final Acceleration by2 = new Acceleration(0.0,
21875 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21876 calibrator.getInitialBiasYAsAcceleration(by2);
21877 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21878 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21879 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21880 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21881 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21882 final Acceleration bz2 = new Acceleration(0.0,
21883 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21884 calibrator.getInitialBiasZAsAcceleration(bz2);
21885 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21886 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21887 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21888 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21889 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21890 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21891 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21892 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21893 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21894 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21895 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21896 final double[] bias1 = calibrator.getInitialBias();
21897 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21898 final double[] bias2 = new double[3];
21899 calibrator.getInitialBias(bias2);
21900 assertArrayEquals(bias1, bias2, 0.0);
21901 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21902 assertEquals(b1, ba);
21903 final Matrix b2 = new Matrix(3, 1);
21904 calibrator.getInitialBiasAsMatrix(b2);
21905 assertEquals(b1, b2);
21906 final Matrix ma1 = calibrator.getInitialMa();
21907 assertEquals(ma1, new Matrix(3, 3));
21908 final Matrix ma2 = new Matrix(3, 3);
21909 calibrator.getInitialMa(ma2);
21910 assertEquals(ma1, ma2);
21911 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21912 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21913 final NEDPosition nedPosition1 = new NEDPosition();
21914 assertTrue(calibrator.getNedPosition(nedPosition1));
21915 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21916 assertSame(calibrator.getMeasurements(), measurements);
21917 assertTrue(calibrator.isCommonAxisUsed());
21918 assertSame(calibrator.getListener(), this);
21919 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21920 assertFalse(calibrator.isReady());
21921 assertFalse(calibrator.isRunning());
21922 assertNull(calibrator.getEstimatedBiases());
21923 assertFalse(calibrator.getEstimatedBiases(null));
21924 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21925 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21926 assertNull(calibrator.getEstimatedBiasFx());
21927 assertNull(calibrator.getEstimatedBiasFy());
21928 assertNull(calibrator.getEstimatedBiasFz());
21929 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21930 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21931 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21932 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21933 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21934 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21935 assertNull(calibrator.getEstimatedMa());
21936 assertNull(calibrator.getEstimatedSx());
21937 assertNull(calibrator.getEstimatedSy());
21938 assertNull(calibrator.getEstimatedSz());
21939 assertNull(calibrator.getEstimatedMxy());
21940 assertNull(calibrator.getEstimatedMxz());
21941 assertNull(calibrator.getEstimatedMyx());
21942 assertNull(calibrator.getEstimatedMyz());
21943 assertNull(calibrator.getEstimatedMzx());
21944 assertNull(calibrator.getEstimatedMzy());
21945 assertNull(calibrator.getEstimatedCovariance());
21946 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21947 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21948 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21949 assertNotNull(calibrator.getGroundTruthGravityNorm());
21950 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21951 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21952 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21953 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21954 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21955 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21956 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21957 }
21958
21959 @Test
21960 public void testConstructor173() throws WrongSizeException {
21961 final Matrix ba = generateBa();
21962 final double biasX = ba.getElementAtIndex(0);
21963 final double biasY = ba.getElementAtIndex(1);
21964 final double biasZ = ba.getElementAtIndex(2);
21965
21966 final Acceleration bx = new Acceleration(biasX,
21967 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21968 final Acceleration by = new Acceleration(biasY,
21969 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21970 final Acceleration bz = new Acceleration(biasZ,
21971 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21972
21973 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21974 final double latitude = Math.toRadians(
21975 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21976 final double longitude = Math.toRadians(
21977 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21978 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21979 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21980 final NEDVelocity nedVelocity = new NEDVelocity();
21981 final ECEFPosition ecefPosition = new ECEFPosition();
21982 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21983 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21984 ecefPosition, ecefVelocity);
21985
21986 final KnownPositionAccelerometerCalibrator calibrator =
21987 new KnownPositionAccelerometerCalibrator(nedPosition,
21988 bx, by, bz);
21989
21990
21991 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21992 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21993 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21994 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21995 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21996 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21997 final Acceleration bx2 = new Acceleration(0.0,
21998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21999 calibrator.getInitialBiasXAsAcceleration(bx2);
22000 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22001 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22002 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22003 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22004 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22005 final Acceleration by2 = new Acceleration(0.0,
22006 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22007 calibrator.getInitialBiasYAsAcceleration(by2);
22008 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22009 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22010 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22011 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22012 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22013 final Acceleration bz2 = new Acceleration(0.0,
22014 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22015 calibrator.getInitialBiasZAsAcceleration(bz2);
22016 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22017 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22018 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22019 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22020 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22021 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22022 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22023 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22024 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22025 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22026 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22027 final double[] bias1 = calibrator.getInitialBias();
22028 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22029 final double[] bias2 = new double[3];
22030 calibrator.getInitialBias(bias2);
22031 assertArrayEquals(bias1, bias2, 0.0);
22032 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22033 assertEquals(b1, ba);
22034 final Matrix b2 = new Matrix(3, 1);
22035 calibrator.getInitialBiasAsMatrix(b2);
22036 assertEquals(b1, b2);
22037 final Matrix ma1 = calibrator.getInitialMa();
22038 assertEquals(ma1, new Matrix(3, 3));
22039 final Matrix ma2 = new Matrix(3, 3);
22040 calibrator.getInitialMa(ma2);
22041 assertEquals(ma1, ma2);
22042 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22043 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22044 final NEDPosition nedPosition1 = new NEDPosition();
22045 assertTrue(calibrator.getNedPosition(nedPosition1));
22046 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22047 assertNull(calibrator.getMeasurements());
22048 assertFalse(calibrator.isCommonAxisUsed());
22049 assertNull(calibrator.getListener());
22050 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22051 assertFalse(calibrator.isReady());
22052 assertFalse(calibrator.isRunning());
22053 assertNull(calibrator.getEstimatedBiases());
22054 assertFalse(calibrator.getEstimatedBiases(null));
22055 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22056 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22057 assertNull(calibrator.getEstimatedBiasFx());
22058 assertNull(calibrator.getEstimatedBiasFy());
22059 assertNull(calibrator.getEstimatedBiasFz());
22060 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22061 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22062 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22063 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22064 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22065 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22066 assertNull(calibrator.getEstimatedMa());
22067 assertNull(calibrator.getEstimatedSx());
22068 assertNull(calibrator.getEstimatedSy());
22069 assertNull(calibrator.getEstimatedSz());
22070 assertNull(calibrator.getEstimatedMxy());
22071 assertNull(calibrator.getEstimatedMxz());
22072 assertNull(calibrator.getEstimatedMyx());
22073 assertNull(calibrator.getEstimatedMyz());
22074 assertNull(calibrator.getEstimatedMzx());
22075 assertNull(calibrator.getEstimatedMzy());
22076 assertNull(calibrator.getEstimatedCovariance());
22077 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22078 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22079 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22080 assertNotNull(calibrator.getGroundTruthGravityNorm());
22081 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22082 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22083 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22084 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22085 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22086 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22087 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22088 }
22089
22090 @Test
22091 public void testConstructor174() throws WrongSizeException {
22092 final Matrix ba = generateBa();
22093 final double biasX = ba.getElementAtIndex(0);
22094 final double biasY = ba.getElementAtIndex(1);
22095 final double biasZ = ba.getElementAtIndex(2);
22096
22097 final Acceleration bx = new Acceleration(biasX,
22098 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22099 final Acceleration by = new Acceleration(biasY,
22100 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22101 final Acceleration bz = new Acceleration(biasZ,
22102 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22103
22104 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22105 final double latitude = Math.toRadians(
22106 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22107 final double longitude = Math.toRadians(
22108 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22109 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22110 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22111 final NEDVelocity nedVelocity = new NEDVelocity();
22112 final ECEFPosition ecefPosition = new ECEFPosition();
22113 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22114 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22115 ecefPosition, ecefVelocity);
22116
22117 final KnownPositionAccelerometerCalibrator calibrator =
22118 new KnownPositionAccelerometerCalibrator(nedPosition,
22119 bx, by, bz, this);
22120
22121
22122 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22123 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22124 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22125 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22126 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22127 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22128 final Acceleration bx2 = new Acceleration(0.0,
22129 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22130 calibrator.getInitialBiasXAsAcceleration(bx2);
22131 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22132 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22133 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22134 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22135 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22136 final Acceleration by2 = new Acceleration(0.0,
22137 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22138 calibrator.getInitialBiasYAsAcceleration(by2);
22139 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22140 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22141 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22142 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22143 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22144 final Acceleration bz2 = new Acceleration(0.0,
22145 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22146 calibrator.getInitialBiasZAsAcceleration(bz2);
22147 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22148 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22149 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22150 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22151 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22152 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22153 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22154 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22155 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22156 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22157 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22158 final double[] bias1 = calibrator.getInitialBias();
22159 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22160 final double[] bias2 = new double[3];
22161 calibrator.getInitialBias(bias2);
22162 assertArrayEquals(bias1, bias2, 0.0);
22163 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22164 assertEquals(b1, ba);
22165 final Matrix b2 = new Matrix(3, 1);
22166 calibrator.getInitialBiasAsMatrix(b2);
22167 assertEquals(b1, b2);
22168 final Matrix ma1 = calibrator.getInitialMa();
22169 assertEquals(ma1, new Matrix(3, 3));
22170 final Matrix ma2 = new Matrix(3, 3);
22171 calibrator.getInitialMa(ma2);
22172 assertEquals(ma1, ma2);
22173 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22174 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22175 final NEDPosition nedPosition1 = new NEDPosition();
22176 assertTrue(calibrator.getNedPosition(nedPosition1));
22177 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22178 assertNull(calibrator.getMeasurements());
22179 assertFalse(calibrator.isCommonAxisUsed());
22180 assertSame(calibrator.getListener(), this);
22181 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22182 assertFalse(calibrator.isReady());
22183 assertFalse(calibrator.isRunning());
22184 assertNull(calibrator.getEstimatedBiases());
22185 assertFalse(calibrator.getEstimatedBiases(null));
22186 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22187 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22188 assertNull(calibrator.getEstimatedBiasFx());
22189 assertNull(calibrator.getEstimatedBiasFy());
22190 assertNull(calibrator.getEstimatedBiasFz());
22191 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22192 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22193 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22194 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22195 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22196 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22197 assertNull(calibrator.getEstimatedMa());
22198 assertNull(calibrator.getEstimatedSx());
22199 assertNull(calibrator.getEstimatedSy());
22200 assertNull(calibrator.getEstimatedSz());
22201 assertNull(calibrator.getEstimatedMxy());
22202 assertNull(calibrator.getEstimatedMxz());
22203 assertNull(calibrator.getEstimatedMyx());
22204 assertNull(calibrator.getEstimatedMyz());
22205 assertNull(calibrator.getEstimatedMzx());
22206 assertNull(calibrator.getEstimatedMzy());
22207 assertNull(calibrator.getEstimatedCovariance());
22208 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22209 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22210 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22211 assertNotNull(calibrator.getGroundTruthGravityNorm());
22212 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22213 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22214 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22215 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22216 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22217 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22218 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22219 }
22220
22221 @Test
22222 public void testConstructor175() throws WrongSizeException {
22223 final Collection<StandardDeviationBodyKinematics> measurements =
22224 Collections.emptyList();
22225
22226 final Matrix ba = generateBa();
22227 final double biasX = ba.getElementAtIndex(0);
22228 final double biasY = ba.getElementAtIndex(1);
22229 final double biasZ = ba.getElementAtIndex(2);
22230
22231 final Acceleration bx = new Acceleration(biasX,
22232 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22233 final Acceleration by = new Acceleration(biasY,
22234 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22235 final Acceleration bz = new Acceleration(biasZ,
22236 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22237
22238 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22239 final double latitude = Math.toRadians(
22240 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22241 final double longitude = Math.toRadians(
22242 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22243 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22244 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22245 final NEDVelocity nedVelocity = new NEDVelocity();
22246 final ECEFPosition ecefPosition = new ECEFPosition();
22247 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22248 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22249 ecefPosition, ecefVelocity);
22250
22251 final KnownPositionAccelerometerCalibrator calibrator =
22252 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22253 bx, by, bz);
22254
22255
22256 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22257 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22258 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22259 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22260 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22261 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22262 final Acceleration bx2 = new Acceleration(0.0,
22263 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22264 calibrator.getInitialBiasXAsAcceleration(bx2);
22265 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22266 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22267 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22268 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22269 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22270 final Acceleration by2 = new Acceleration(0.0,
22271 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22272 calibrator.getInitialBiasYAsAcceleration(by2);
22273 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22274 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22275 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22276 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22277 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22278 final Acceleration bz2 = new Acceleration(0.0,
22279 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22280 calibrator.getInitialBiasZAsAcceleration(bz2);
22281 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22282 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22283 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22284 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22285 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22286 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22287 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22288 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22289 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22290 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22291 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22292 final double[] bias1 = calibrator.getInitialBias();
22293 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22294 final double[] bias2 = new double[3];
22295 calibrator.getInitialBias(bias2);
22296 assertArrayEquals(bias1, bias2, 0.0);
22297 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22298 assertEquals(b1, ba);
22299 final Matrix b2 = new Matrix(3, 1);
22300 calibrator.getInitialBiasAsMatrix(b2);
22301 assertEquals(b1, b2);
22302 final Matrix ma1 = calibrator.getInitialMa();
22303 assertEquals(ma1, new Matrix(3, 3));
22304 final Matrix ma2 = new Matrix(3, 3);
22305 calibrator.getInitialMa(ma2);
22306 assertEquals(ma1, ma2);
22307 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22308 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22309 final NEDPosition nedPosition1 = new NEDPosition();
22310 assertTrue(calibrator.getNedPosition(nedPosition1));
22311 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22312 assertSame(calibrator.getMeasurements(), measurements);
22313 assertFalse(calibrator.isCommonAxisUsed());
22314 assertNull(calibrator.getListener());
22315 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22316 assertFalse(calibrator.isReady());
22317 assertFalse(calibrator.isRunning());
22318 assertNull(calibrator.getEstimatedBiases());
22319 assertFalse(calibrator.getEstimatedBiases(null));
22320 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22321 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22322 assertNull(calibrator.getEstimatedBiasFx());
22323 assertNull(calibrator.getEstimatedBiasFy());
22324 assertNull(calibrator.getEstimatedBiasFz());
22325 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22326 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22327 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22328 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22329 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22330 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22331 assertNull(calibrator.getEstimatedMa());
22332 assertNull(calibrator.getEstimatedSx());
22333 assertNull(calibrator.getEstimatedSy());
22334 assertNull(calibrator.getEstimatedSz());
22335 assertNull(calibrator.getEstimatedMxy());
22336 assertNull(calibrator.getEstimatedMxz());
22337 assertNull(calibrator.getEstimatedMyx());
22338 assertNull(calibrator.getEstimatedMyz());
22339 assertNull(calibrator.getEstimatedMzx());
22340 assertNull(calibrator.getEstimatedMzy());
22341 assertNull(calibrator.getEstimatedCovariance());
22342 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22343 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22344 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22345 assertNotNull(calibrator.getGroundTruthGravityNorm());
22346 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22347 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22348 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22349 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22350 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22351 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22352 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22353 }
22354
22355 @Test
22356 public void testConstructor176() throws WrongSizeException {
22357 final Collection<StandardDeviationBodyKinematics> measurements =
22358 Collections.emptyList();
22359
22360 final Matrix ba = generateBa();
22361 final double biasX = ba.getElementAtIndex(0);
22362 final double biasY = ba.getElementAtIndex(1);
22363 final double biasZ = ba.getElementAtIndex(2);
22364
22365 final Acceleration bx = new Acceleration(biasX,
22366 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22367 final Acceleration by = new Acceleration(biasY,
22368 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22369 final Acceleration bz = new Acceleration(biasZ,
22370 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22371
22372 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22373 final double latitude = Math.toRadians(
22374 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22375 final double longitude = Math.toRadians(
22376 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22377 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22378 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22379 final NEDVelocity nedVelocity = new NEDVelocity();
22380 final ECEFPosition ecefPosition = new ECEFPosition();
22381 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22382 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22383 ecefPosition, ecefVelocity);
22384
22385 final KnownPositionAccelerometerCalibrator calibrator =
22386 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22387 bx, by, bz, this);
22388
22389
22390 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22391 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22392 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22393 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22394 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22395 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22396 final Acceleration bx2 = new Acceleration(0.0,
22397 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22398 calibrator.getInitialBiasXAsAcceleration(bx2);
22399 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22400 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22401 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22402 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22403 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22404 final Acceleration by2 = new Acceleration(0.0,
22405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22406 calibrator.getInitialBiasYAsAcceleration(by2);
22407 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22408 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22409 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22410 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22411 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22412 final Acceleration bz2 = new Acceleration(0.0,
22413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22414 calibrator.getInitialBiasZAsAcceleration(bz2);
22415 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22416 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22417 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22418 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22419 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22420 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22421 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22422 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22423 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22424 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22425 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22426 final double[] bias1 = calibrator.getInitialBias();
22427 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22428 final double[] bias2 = new double[3];
22429 calibrator.getInitialBias(bias2);
22430 assertArrayEquals(bias1, bias2, 0.0);
22431 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22432 assertEquals(b1, ba);
22433 final Matrix b2 = new Matrix(3, 1);
22434 calibrator.getInitialBiasAsMatrix(b2);
22435 assertEquals(b1, b2);
22436 final Matrix ma1 = calibrator.getInitialMa();
22437 assertEquals(ma1, new Matrix(3, 3));
22438 final Matrix ma2 = new Matrix(3, 3);
22439 calibrator.getInitialMa(ma2);
22440 assertEquals(ma1, ma2);
22441 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22442 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22443 final NEDPosition nedPosition1 = new NEDPosition();
22444 assertTrue(calibrator.getNedPosition(nedPosition1));
22445 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22446 assertSame(calibrator.getMeasurements(), measurements);
22447 assertFalse(calibrator.isCommonAxisUsed());
22448 assertSame(calibrator.getListener(), this);
22449 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22450 assertFalse(calibrator.isReady());
22451 assertFalse(calibrator.isRunning());
22452 assertNull(calibrator.getEstimatedBiases());
22453 assertFalse(calibrator.getEstimatedBiases(null));
22454 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22455 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22456 assertNull(calibrator.getEstimatedBiasFx());
22457 assertNull(calibrator.getEstimatedBiasFy());
22458 assertNull(calibrator.getEstimatedBiasFz());
22459 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22460 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22461 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22462 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22463 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22464 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22465 assertNull(calibrator.getEstimatedMa());
22466 assertNull(calibrator.getEstimatedSx());
22467 assertNull(calibrator.getEstimatedSy());
22468 assertNull(calibrator.getEstimatedSz());
22469 assertNull(calibrator.getEstimatedMxy());
22470 assertNull(calibrator.getEstimatedMxz());
22471 assertNull(calibrator.getEstimatedMyx());
22472 assertNull(calibrator.getEstimatedMyz());
22473 assertNull(calibrator.getEstimatedMzx());
22474 assertNull(calibrator.getEstimatedMzy());
22475 assertNull(calibrator.getEstimatedCovariance());
22476 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22477 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22478 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22479 assertNotNull(calibrator.getGroundTruthGravityNorm());
22480 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22481 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22482 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22483 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22484 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22485 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22486 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22487 }
22488
22489 @Test
22490 public void testConstructor177() throws WrongSizeException {
22491 final Matrix ba = generateBa();
22492 final double biasX = ba.getElementAtIndex(0);
22493 final double biasY = ba.getElementAtIndex(1);
22494 final double biasZ = ba.getElementAtIndex(2);
22495
22496 final Acceleration bx = new Acceleration(biasX,
22497 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22498 final Acceleration by = new Acceleration(biasY,
22499 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22500 final Acceleration bz = new Acceleration(biasZ,
22501 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22502
22503 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22504 final double latitude = Math.toRadians(
22505 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22506 final double longitude = Math.toRadians(
22507 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22508 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22509 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22510 final NEDVelocity nedVelocity = new NEDVelocity();
22511 final ECEFPosition ecefPosition = new ECEFPosition();
22512 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22513 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22514 ecefPosition, ecefVelocity);
22515
22516 final KnownPositionAccelerometerCalibrator calibrator =
22517 new KnownPositionAccelerometerCalibrator(nedPosition,
22518 true, bx, by, bz);
22519
22520
22521 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22522 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22523 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22524 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22525 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22526 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22527 final Acceleration bx2 = new Acceleration(0.0,
22528 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22529 calibrator.getInitialBiasXAsAcceleration(bx2);
22530 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22531 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22532 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22533 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22534 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22535 final Acceleration by2 = new Acceleration(0.0,
22536 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22537 calibrator.getInitialBiasYAsAcceleration(by2);
22538 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22539 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22540 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22541 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22542 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22543 final Acceleration bz2 = new Acceleration(0.0,
22544 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22545 calibrator.getInitialBiasZAsAcceleration(bz2);
22546 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22547 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22548 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22549 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22550 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22551 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22552 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22553 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22554 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22555 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22556 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22557 final double[] bias1 = calibrator.getInitialBias();
22558 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22559 final double[] bias2 = new double[3];
22560 calibrator.getInitialBias(bias2);
22561 assertArrayEquals(bias1, bias2, 0.0);
22562 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22563 assertEquals(b1, ba);
22564 final Matrix b2 = new Matrix(3, 1);
22565 calibrator.getInitialBiasAsMatrix(b2);
22566 assertEquals(b1, b2);
22567 final Matrix ma1 = calibrator.getInitialMa();
22568 assertEquals(ma1, new Matrix(3, 3));
22569 final Matrix ma2 = new Matrix(3, 3);
22570 calibrator.getInitialMa(ma2);
22571 assertEquals(ma1, ma2);
22572 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22573 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22574 final NEDPosition nedPosition1 = new NEDPosition();
22575 assertTrue(calibrator.getNedPosition(nedPosition1));
22576 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22577 assertNull(calibrator.getMeasurements());
22578 assertTrue(calibrator.isCommonAxisUsed());
22579 assertNull(calibrator.getListener());
22580 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22581 assertFalse(calibrator.isReady());
22582 assertFalse(calibrator.isRunning());
22583 assertNull(calibrator.getEstimatedBiases());
22584 assertFalse(calibrator.getEstimatedBiases(null));
22585 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22586 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22587 assertNull(calibrator.getEstimatedBiasFx());
22588 assertNull(calibrator.getEstimatedBiasFy());
22589 assertNull(calibrator.getEstimatedBiasFz());
22590 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22591 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22592 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22593 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22594 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22595 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22596 assertNull(calibrator.getEstimatedMa());
22597 assertNull(calibrator.getEstimatedSx());
22598 assertNull(calibrator.getEstimatedSy());
22599 assertNull(calibrator.getEstimatedSz());
22600 assertNull(calibrator.getEstimatedMxy());
22601 assertNull(calibrator.getEstimatedMxz());
22602 assertNull(calibrator.getEstimatedMyx());
22603 assertNull(calibrator.getEstimatedMyz());
22604 assertNull(calibrator.getEstimatedMzx());
22605 assertNull(calibrator.getEstimatedMzy());
22606 assertNull(calibrator.getEstimatedCovariance());
22607 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22608 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22609 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22610 assertNotNull(calibrator.getGroundTruthGravityNorm());
22611 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22612 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22613 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22614 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22615 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22616 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22617 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22618 }
22619
22620 @Test
22621 public void testConstructor178() throws WrongSizeException {
22622 final Matrix ba = generateBa();
22623 final double biasX = ba.getElementAtIndex(0);
22624 final double biasY = ba.getElementAtIndex(1);
22625 final double biasZ = ba.getElementAtIndex(2);
22626
22627 final Acceleration bx = new Acceleration(biasX,
22628 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22629 final Acceleration by = new Acceleration(biasY,
22630 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22631 final Acceleration bz = new Acceleration(biasZ,
22632 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22633
22634 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22635 final double latitude = Math.toRadians(
22636 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22637 final double longitude = Math.toRadians(
22638 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22639 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22640 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22641 final NEDVelocity nedVelocity = new NEDVelocity();
22642 final ECEFPosition ecefPosition = new ECEFPosition();
22643 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22644 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22645 ecefPosition, ecefVelocity);
22646
22647 final KnownPositionAccelerometerCalibrator calibrator =
22648 new KnownPositionAccelerometerCalibrator(nedPosition,
22649 true, bx, by, bz, this);
22650
22651
22652 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22653 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22654 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22655 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22656 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22657 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22658 final Acceleration bx2 = new Acceleration(0.0,
22659 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22660 calibrator.getInitialBiasXAsAcceleration(bx2);
22661 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22662 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22663 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22664 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22665 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22666 final Acceleration by2 = new Acceleration(0.0,
22667 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22668 calibrator.getInitialBiasYAsAcceleration(by2);
22669 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22670 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22671 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22672 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22673 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22674 final Acceleration bz2 = new Acceleration(0.0,
22675 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22676 calibrator.getInitialBiasZAsAcceleration(bz2);
22677 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22678 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22679 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22680 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22681 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22682 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22683 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22684 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22685 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22686 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22687 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22688 final double[] bias1 = calibrator.getInitialBias();
22689 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22690 final double[] bias2 = new double[3];
22691 calibrator.getInitialBias(bias2);
22692 assertArrayEquals(bias1, bias2, 0.0);
22693 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22694 assertEquals(b1, ba);
22695 final Matrix b2 = new Matrix(3, 1);
22696 calibrator.getInitialBiasAsMatrix(b2);
22697 assertEquals(b1, b2);
22698 final Matrix ma1 = calibrator.getInitialMa();
22699 assertEquals(ma1, new Matrix(3, 3));
22700 final Matrix ma2 = new Matrix(3, 3);
22701 calibrator.getInitialMa(ma2);
22702 assertEquals(ma1, ma2);
22703 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22704 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22705 final NEDPosition nedPosition1 = new NEDPosition();
22706 assertTrue(calibrator.getNedPosition(nedPosition1));
22707 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22708 assertNull(calibrator.getMeasurements());
22709 assertTrue(calibrator.isCommonAxisUsed());
22710 assertSame(calibrator.getListener(), this);
22711 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22712 assertFalse(calibrator.isReady());
22713 assertFalse(calibrator.isRunning());
22714 assertNull(calibrator.getEstimatedBiases());
22715 assertFalse(calibrator.getEstimatedBiases(null));
22716 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22717 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22718 assertNull(calibrator.getEstimatedBiasFx());
22719 assertNull(calibrator.getEstimatedBiasFy());
22720 assertNull(calibrator.getEstimatedBiasFz());
22721 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22722 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22723 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22724 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22725 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22726 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22727 assertNull(calibrator.getEstimatedMa());
22728 assertNull(calibrator.getEstimatedSx());
22729 assertNull(calibrator.getEstimatedSy());
22730 assertNull(calibrator.getEstimatedSz());
22731 assertNull(calibrator.getEstimatedMxy());
22732 assertNull(calibrator.getEstimatedMxz());
22733 assertNull(calibrator.getEstimatedMyx());
22734 assertNull(calibrator.getEstimatedMyz());
22735 assertNull(calibrator.getEstimatedMzx());
22736 assertNull(calibrator.getEstimatedMzy());
22737 assertNull(calibrator.getEstimatedCovariance());
22738 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22739 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22740 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22741 assertNotNull(calibrator.getGroundTruthGravityNorm());
22742 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22743 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22744 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22745 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22746 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22747 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22748 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22749 }
22750
22751 @Test
22752 public void testConstructor179() throws WrongSizeException {
22753 final Collection<StandardDeviationBodyKinematics> measurements =
22754 Collections.emptyList();
22755
22756 final Matrix ba = generateBa();
22757 final double biasX = ba.getElementAtIndex(0);
22758 final double biasY = ba.getElementAtIndex(1);
22759 final double biasZ = ba.getElementAtIndex(2);
22760
22761 final Acceleration bx = new Acceleration(biasX,
22762 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22763 final Acceleration by = new Acceleration(biasY,
22764 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22765 final Acceleration bz = new Acceleration(biasZ,
22766 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22767
22768 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22769 final double latitude = Math.toRadians(
22770 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22771 final double longitude = Math.toRadians(
22772 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22773 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22774 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22775 final NEDVelocity nedVelocity = new NEDVelocity();
22776 final ECEFPosition ecefPosition = new ECEFPosition();
22777 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22778 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22779 ecefPosition, ecefVelocity);
22780
22781 final KnownPositionAccelerometerCalibrator calibrator =
22782 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22783 true, bx, by, bz);
22784
22785
22786 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22787 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22788 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22789 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22790 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22791 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22792 final Acceleration bx2 = new Acceleration(0.0,
22793 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22794 calibrator.getInitialBiasXAsAcceleration(bx2);
22795 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22796 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22797 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22798 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22799 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22800 final Acceleration by2 = new Acceleration(0.0,
22801 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22802 calibrator.getInitialBiasYAsAcceleration(by2);
22803 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22804 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22805 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22806 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22807 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22808 final Acceleration bz2 = new Acceleration(0.0,
22809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22810 calibrator.getInitialBiasZAsAcceleration(bz2);
22811 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22812 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22813 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22814 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22815 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22816 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22817 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22818 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22819 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22820 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22821 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22822 final double[] bias1 = calibrator.getInitialBias();
22823 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22824 final double[] bias2 = new double[3];
22825 calibrator.getInitialBias(bias2);
22826 assertArrayEquals(bias1, bias2, 0.0);
22827 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22828 assertEquals(b1, ba);
22829 final Matrix b2 = new Matrix(3, 1);
22830 calibrator.getInitialBiasAsMatrix(b2);
22831 assertEquals(b1, b2);
22832 final Matrix ma1 = calibrator.getInitialMa();
22833 assertEquals(ma1, new Matrix(3, 3));
22834 final Matrix ma2 = new Matrix(3, 3);
22835 calibrator.getInitialMa(ma2);
22836 assertEquals(ma1, ma2);
22837 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22838 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22839 final NEDPosition nedPosition1 = new NEDPosition();
22840 assertTrue(calibrator.getNedPosition(nedPosition1));
22841 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22842 assertSame(calibrator.getMeasurements(), measurements);
22843 assertTrue(calibrator.isCommonAxisUsed());
22844 assertNull(calibrator.getListener());
22845 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22846 assertFalse(calibrator.isReady());
22847 assertFalse(calibrator.isRunning());
22848 assertNull(calibrator.getEstimatedBiases());
22849 assertFalse(calibrator.getEstimatedBiases(null));
22850 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22851 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22852 assertNull(calibrator.getEstimatedBiasFx());
22853 assertNull(calibrator.getEstimatedBiasFy());
22854 assertNull(calibrator.getEstimatedBiasFz());
22855 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22856 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22857 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22858 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22859 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22860 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22861 assertNull(calibrator.getEstimatedMa());
22862 assertNull(calibrator.getEstimatedSx());
22863 assertNull(calibrator.getEstimatedSy());
22864 assertNull(calibrator.getEstimatedSz());
22865 assertNull(calibrator.getEstimatedMxy());
22866 assertNull(calibrator.getEstimatedMxz());
22867 assertNull(calibrator.getEstimatedMyx());
22868 assertNull(calibrator.getEstimatedMyz());
22869 assertNull(calibrator.getEstimatedMzx());
22870 assertNull(calibrator.getEstimatedMzy());
22871 assertNull(calibrator.getEstimatedCovariance());
22872 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22873 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22874 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22875 assertNotNull(calibrator.getGroundTruthGravityNorm());
22876 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22877 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22878 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22879 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22880 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22881 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22882 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22883 }
22884
22885 @Test
22886 public void testConstructor180() throws WrongSizeException {
22887 final Collection<StandardDeviationBodyKinematics> measurements =
22888 Collections.emptyList();
22889
22890 final Matrix ba = generateBa();
22891 final double biasX = ba.getElementAtIndex(0);
22892 final double biasY = ba.getElementAtIndex(1);
22893 final double biasZ = ba.getElementAtIndex(2);
22894
22895 final Acceleration bx = new Acceleration(biasX,
22896 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22897 final Acceleration by = new Acceleration(biasY,
22898 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22899 final Acceleration bz = new Acceleration(biasZ,
22900 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22901
22902 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22903 final double latitude = Math.toRadians(
22904 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22905 final double longitude = Math.toRadians(
22906 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22907 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22908 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22909 final NEDVelocity nedVelocity = new NEDVelocity();
22910 final ECEFPosition ecefPosition = new ECEFPosition();
22911 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22912 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22913 ecefPosition, ecefVelocity);
22914
22915 final KnownPositionAccelerometerCalibrator calibrator =
22916 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22917 true, bx, by, bz, this);
22918
22919
22920 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22921 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22922 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22923 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22924 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22925 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22926 final Acceleration bx2 = new Acceleration(0.0,
22927 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22928 calibrator.getInitialBiasXAsAcceleration(bx2);
22929 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22930 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22931 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22932 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22933 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22934 final Acceleration by2 = new Acceleration(0.0,
22935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22936 calibrator.getInitialBiasYAsAcceleration(by2);
22937 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22938 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22939 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22940 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22941 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22942 final Acceleration bz2 = new Acceleration(0.0,
22943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22944 calibrator.getInitialBiasZAsAcceleration(bz2);
22945 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22946 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22947 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22948 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22949 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22950 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22951 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22952 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22953 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22954 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22955 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22956 final double[] bias1 = calibrator.getInitialBias();
22957 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22958 final double[] bias2 = new double[3];
22959 calibrator.getInitialBias(bias2);
22960 assertArrayEquals(bias1, bias2, 0.0);
22961 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22962 assertEquals(b1, ba);
22963 final Matrix b2 = new Matrix(3, 1);
22964 calibrator.getInitialBiasAsMatrix(b2);
22965 assertEquals(b1, b2);
22966 final Matrix ma1 = calibrator.getInitialMa();
22967 assertEquals(ma1, new Matrix(3, 3));
22968 final Matrix ma2 = new Matrix(3, 3);
22969 calibrator.getInitialMa(ma2);
22970 assertEquals(ma1, ma2);
22971 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22972 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22973 final NEDPosition nedPosition1 = new NEDPosition();
22974 assertTrue(calibrator.getNedPosition(nedPosition1));
22975 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22976 assertSame(calibrator.getMeasurements(), measurements);
22977 assertTrue(calibrator.isCommonAxisUsed());
22978 assertSame(calibrator.getListener(), this);
22979 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22980 assertFalse(calibrator.isReady());
22981 assertFalse(calibrator.isRunning());
22982 assertNull(calibrator.getEstimatedBiases());
22983 assertFalse(calibrator.getEstimatedBiases(null));
22984 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22985 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22986 assertNull(calibrator.getEstimatedBiasFx());
22987 assertNull(calibrator.getEstimatedBiasFy());
22988 assertNull(calibrator.getEstimatedBiasFz());
22989 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22990 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22991 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22992 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22993 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22994 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22995 assertNull(calibrator.getEstimatedMa());
22996 assertNull(calibrator.getEstimatedSx());
22997 assertNull(calibrator.getEstimatedSy());
22998 assertNull(calibrator.getEstimatedSz());
22999 assertNull(calibrator.getEstimatedMxy());
23000 assertNull(calibrator.getEstimatedMxz());
23001 assertNull(calibrator.getEstimatedMyx());
23002 assertNull(calibrator.getEstimatedMyz());
23003 assertNull(calibrator.getEstimatedMzx());
23004 assertNull(calibrator.getEstimatedMzy());
23005 assertNull(calibrator.getEstimatedCovariance());
23006 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23007 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23008 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23009 assertNotNull(calibrator.getGroundTruthGravityNorm());
23010 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23011 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23012 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23013 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23014 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23015 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23016 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23017 }
23018
23019 @Test
23020 public void testConstructor181() throws WrongSizeException {
23021 final Matrix ba = generateBa();
23022 final double biasX = ba.getElementAtIndex(0);
23023 final double biasY = ba.getElementAtIndex(1);
23024 final double biasZ = ba.getElementAtIndex(2);
23025
23026 final Matrix ma = generateMaCommonAxis();
23027 final double sx = ma.getElementAt(0, 0);
23028 final double sy = ma.getElementAt(1, 1);
23029 final double sz = ma.getElementAt(2, 2);
23030
23031 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23032 final double latitude = Math.toRadians(
23033 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23034 final double longitude = Math.toRadians(
23035 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23036 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23037 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23038 final NEDVelocity nedVelocity = new NEDVelocity();
23039 final ECEFPosition ecefPosition = new ECEFPosition();
23040 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23041 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23042 ecefPosition, ecefVelocity);
23043
23044 final KnownPositionAccelerometerCalibrator calibrator =
23045 new KnownPositionAccelerometerCalibrator(nedPosition,
23046 biasX, biasY, biasZ, sx, sy, sz);
23047
23048
23049 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23050 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23051 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23052 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23053 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23054 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23055 final Acceleration bx2 = new Acceleration(0.0,
23056 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23057 calibrator.getInitialBiasXAsAcceleration(bx2);
23058 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23059 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23060 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23061 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23062 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23063 final Acceleration by2 = new Acceleration(0.0,
23064 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23065 calibrator.getInitialBiasYAsAcceleration(by2);
23066 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23067 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23068 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23069 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23070 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23071 final Acceleration bz2 = new Acceleration(0.0,
23072 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23073 calibrator.getInitialBiasZAsAcceleration(bz2);
23074 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23075 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23076 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23077 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23078 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23079 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23080 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23081 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23082 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23083 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23084 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23085 final double[] bias1 = calibrator.getInitialBias();
23086 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23087 final double[] bias2 = new double[3];
23088 calibrator.getInitialBias(bias2);
23089 assertArrayEquals(bias1, bias2, 0.0);
23090 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23091 assertEquals(b1, ba);
23092 final Matrix b2 = new Matrix(3, 1);
23093 calibrator.getInitialBiasAsMatrix(b2);
23094 assertEquals(b1, b2);
23095 final Matrix ma1 = calibrator.getInitialMa();
23096 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23097 final Matrix ma2 = new Matrix(3, 3);
23098 calibrator.getInitialMa(ma2);
23099 assertEquals(ma1, ma2);
23100 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23101 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23102 final NEDPosition nedPosition1 = new NEDPosition();
23103 assertTrue(calibrator.getNedPosition(nedPosition1));
23104 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23105 assertNull(calibrator.getMeasurements());
23106 assertFalse(calibrator.isCommonAxisUsed());
23107 assertNull(calibrator.getListener());
23108 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23109 assertFalse(calibrator.isReady());
23110 assertFalse(calibrator.isRunning());
23111 assertNull(calibrator.getEstimatedBiases());
23112 assertFalse(calibrator.getEstimatedBiases(null));
23113 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23114 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23115 assertNull(calibrator.getEstimatedBiasFx());
23116 assertNull(calibrator.getEstimatedBiasFy());
23117 assertNull(calibrator.getEstimatedBiasFz());
23118 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23119 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23120 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23121 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23122 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23123 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23124 assertNull(calibrator.getEstimatedMa());
23125 assertNull(calibrator.getEstimatedSx());
23126 assertNull(calibrator.getEstimatedSy());
23127 assertNull(calibrator.getEstimatedSz());
23128 assertNull(calibrator.getEstimatedMxy());
23129 assertNull(calibrator.getEstimatedMxz());
23130 assertNull(calibrator.getEstimatedMyx());
23131 assertNull(calibrator.getEstimatedMyz());
23132 assertNull(calibrator.getEstimatedMzx());
23133 assertNull(calibrator.getEstimatedMzy());
23134 assertNull(calibrator.getEstimatedCovariance());
23135 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23136 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23137 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23138 assertNotNull(calibrator.getGroundTruthGravityNorm());
23139 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23140 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23141 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23142 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23143 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23144 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23145 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23146 }
23147
23148 @Test
23149 public void testConstructor182() throws WrongSizeException {
23150 final Collection<StandardDeviationBodyKinematics> measurements =
23151 Collections.emptyList();
23152
23153 final Matrix ba = generateBa();
23154 final double biasX = ba.getElementAtIndex(0);
23155 final double biasY = ba.getElementAtIndex(1);
23156 final double biasZ = ba.getElementAtIndex(2);
23157
23158 final Matrix ma = generateMaCommonAxis();
23159 final double sx = ma.getElementAt(0, 0);
23160 final double sy = ma.getElementAt(1, 1);
23161 final double sz = ma.getElementAt(2, 2);
23162
23163 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23164 final double latitude = Math.toRadians(
23165 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23166 final double longitude = Math.toRadians(
23167 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23168 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23169 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23170 final NEDVelocity nedVelocity = new NEDVelocity();
23171 final ECEFPosition ecefPosition = new ECEFPosition();
23172 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23173 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23174 ecefPosition, ecefVelocity);
23175
23176 final KnownPositionAccelerometerCalibrator calibrator =
23177 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23178 biasX, biasY, biasZ, sx, sy, sz);
23179
23180
23181 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23182 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23183 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23184 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23185 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23186 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23187 final Acceleration bx2 = new Acceleration(0.0,
23188 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23189 calibrator.getInitialBiasXAsAcceleration(bx2);
23190 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23191 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23192 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23193 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23194 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23195 final Acceleration by2 = new Acceleration(0.0,
23196 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23197 calibrator.getInitialBiasYAsAcceleration(by2);
23198 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23199 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23200 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23201 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23202 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23203 final Acceleration bz2 = new Acceleration(0.0,
23204 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23205 calibrator.getInitialBiasZAsAcceleration(bz2);
23206 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23207 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23208 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23209 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23210 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23211 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23212 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23213 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23214 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23215 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23216 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23217 final double[] bias1 = calibrator.getInitialBias();
23218 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23219 final double[] bias2 = new double[3];
23220 calibrator.getInitialBias(bias2);
23221 assertArrayEquals(bias1, bias2, 0.0);
23222 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23223 assertEquals(b1, ba);
23224 final Matrix b2 = new Matrix(3, 1);
23225 calibrator.getInitialBiasAsMatrix(b2);
23226 assertEquals(b1, b2);
23227 final Matrix ma1 = calibrator.getInitialMa();
23228 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23229 final Matrix ma2 = new Matrix(3, 3);
23230 calibrator.getInitialMa(ma2);
23231 assertEquals(ma1, ma2);
23232 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23233 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23234 final NEDPosition nedPosition1 = new NEDPosition();
23235 assertTrue(calibrator.getNedPosition(nedPosition1));
23236 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23237 assertSame(calibrator.getMeasurements(), measurements);
23238 assertFalse(calibrator.isCommonAxisUsed());
23239 assertNull(calibrator.getListener());
23240 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23241 assertFalse(calibrator.isReady());
23242 assertFalse(calibrator.isRunning());
23243 assertNull(calibrator.getEstimatedBiases());
23244 assertFalse(calibrator.getEstimatedBiases(null));
23245 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23246 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23247 assertNull(calibrator.getEstimatedBiasFx());
23248 assertNull(calibrator.getEstimatedBiasFy());
23249 assertNull(calibrator.getEstimatedBiasFz());
23250 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23251 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23252 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23253 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23254 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23255 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23256 assertNull(calibrator.getEstimatedMa());
23257 assertNull(calibrator.getEstimatedSx());
23258 assertNull(calibrator.getEstimatedSy());
23259 assertNull(calibrator.getEstimatedSz());
23260 assertNull(calibrator.getEstimatedMxy());
23261 assertNull(calibrator.getEstimatedMxz());
23262 assertNull(calibrator.getEstimatedMyx());
23263 assertNull(calibrator.getEstimatedMyz());
23264 assertNull(calibrator.getEstimatedMzx());
23265 assertNull(calibrator.getEstimatedMzy());
23266 assertNull(calibrator.getEstimatedCovariance());
23267 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23268 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23269 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23270 assertNotNull(calibrator.getGroundTruthGravityNorm());
23271 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23272 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23273 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23274 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23275 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23276 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23277 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23278 }
23279
23280 @Test
23281 public void testConstructor183() throws WrongSizeException {
23282 final Collection<StandardDeviationBodyKinematics> measurements =
23283 Collections.emptyList();
23284
23285 final Matrix ba = generateBa();
23286 final double biasX = ba.getElementAtIndex(0);
23287 final double biasY = ba.getElementAtIndex(1);
23288 final double biasZ = ba.getElementAtIndex(2);
23289
23290 final Matrix ma = generateMaCommonAxis();
23291 final double sx = ma.getElementAt(0, 0);
23292 final double sy = ma.getElementAt(1, 1);
23293 final double sz = ma.getElementAt(2, 2);
23294
23295 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23296 final double latitude = Math.toRadians(
23297 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23298 final double longitude = Math.toRadians(
23299 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23300 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23301 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23302 final NEDVelocity nedVelocity = new NEDVelocity();
23303 final ECEFPosition ecefPosition = new ECEFPosition();
23304 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23305 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23306 ecefPosition, ecefVelocity);
23307
23308 final KnownPositionAccelerometerCalibrator calibrator =
23309 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23310 biasX, biasY, biasZ, sx, sy, sz, this);
23311
23312
23313 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23314 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23315 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23316 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23317 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23318 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23319 final Acceleration bx2 = new Acceleration(0.0,
23320 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23321 calibrator.getInitialBiasXAsAcceleration(bx2);
23322 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23323 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23324 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23325 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23326 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23327 final Acceleration by2 = new Acceleration(0.0,
23328 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23329 calibrator.getInitialBiasYAsAcceleration(by2);
23330 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23331 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23332 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23333 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23334 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23335 final Acceleration bz2 = new Acceleration(0.0,
23336 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23337 calibrator.getInitialBiasZAsAcceleration(bz2);
23338 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23339 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23340 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23341 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23342 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23343 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23344 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23345 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23346 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23347 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23348 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23349 final double[] bias1 = calibrator.getInitialBias();
23350 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23351 final double[] bias2 = new double[3];
23352 calibrator.getInitialBias(bias2);
23353 assertArrayEquals(bias1, bias2, 0.0);
23354 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23355 assertEquals(b1, ba);
23356 final Matrix b2 = new Matrix(3, 1);
23357 calibrator.getInitialBiasAsMatrix(b2);
23358 assertEquals(b1, b2);
23359 final Matrix ma1 = calibrator.getInitialMa();
23360 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23361 final Matrix ma2 = new Matrix(3, 3);
23362 calibrator.getInitialMa(ma2);
23363 assertEquals(ma1, ma2);
23364 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23365 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23366 final NEDPosition nedPosition1 = new NEDPosition();
23367 assertTrue(calibrator.getNedPosition(nedPosition1));
23368 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23369 assertSame(calibrator.getMeasurements(), measurements);
23370 assertFalse(calibrator.isCommonAxisUsed());
23371 assertSame(calibrator.getListener(), this);
23372 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23373 assertFalse(calibrator.isReady());
23374 assertFalse(calibrator.isRunning());
23375 assertNull(calibrator.getEstimatedBiases());
23376 assertFalse(calibrator.getEstimatedBiases(null));
23377 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23378 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23379 assertNull(calibrator.getEstimatedBiasFx());
23380 assertNull(calibrator.getEstimatedBiasFy());
23381 assertNull(calibrator.getEstimatedBiasFz());
23382 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23383 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23384 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23385 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23386 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23387 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23388 assertNull(calibrator.getEstimatedMa());
23389 assertNull(calibrator.getEstimatedSx());
23390 assertNull(calibrator.getEstimatedSy());
23391 assertNull(calibrator.getEstimatedSz());
23392 assertNull(calibrator.getEstimatedMxy());
23393 assertNull(calibrator.getEstimatedMxz());
23394 assertNull(calibrator.getEstimatedMyx());
23395 assertNull(calibrator.getEstimatedMyz());
23396 assertNull(calibrator.getEstimatedMzx());
23397 assertNull(calibrator.getEstimatedMzy());
23398 assertNull(calibrator.getEstimatedCovariance());
23399 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23400 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23401 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23402 assertNotNull(calibrator.getGroundTruthGravityNorm());
23403 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23404 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23405 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23406 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23407 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23408 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23409 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23410 }
23411
23412 @Test
23413 public void testConstructor184() throws WrongSizeException {
23414 final Matrix ba = generateBa();
23415 final double biasX = ba.getElementAtIndex(0);
23416 final double biasY = ba.getElementAtIndex(1);
23417 final double biasZ = ba.getElementAtIndex(2);
23418
23419 final Matrix ma = generateMaCommonAxis();
23420 final double sx = ma.getElementAt(0, 0);
23421 final double sy = ma.getElementAt(1, 1);
23422 final double sz = ma.getElementAt(2, 2);
23423
23424 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23425 final double latitude = Math.toRadians(
23426 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23427 final double longitude = Math.toRadians(
23428 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23429 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23430 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23431 final NEDVelocity nedVelocity = new NEDVelocity();
23432 final ECEFPosition ecefPosition = new ECEFPosition();
23433 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23434 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23435 ecefPosition, ecefVelocity);
23436
23437 final KnownPositionAccelerometerCalibrator calibrator =
23438 new KnownPositionAccelerometerCalibrator(nedPosition,
23439 true, biasX, biasY, biasZ, sx, sy, sz);
23440
23441
23442 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23443 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23444 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23445 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23446 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23447 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23448 final Acceleration bx2 = new Acceleration(0.0,
23449 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23450 calibrator.getInitialBiasXAsAcceleration(bx2);
23451 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23452 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23453 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23454 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23455 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23456 final Acceleration by2 = new Acceleration(0.0,
23457 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23458 calibrator.getInitialBiasYAsAcceleration(by2);
23459 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23460 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23461 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23462 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23463 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23464 final Acceleration bz2 = new Acceleration(0.0,
23465 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23466 calibrator.getInitialBiasZAsAcceleration(bz2);
23467 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23468 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23469 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23470 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23471 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23472 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23473 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23474 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23475 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23476 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23477 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23478 final double[] bias1 = calibrator.getInitialBias();
23479 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23480 final double[] bias2 = new double[3];
23481 calibrator.getInitialBias(bias2);
23482 assertArrayEquals(bias1, bias2, 0.0);
23483 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23484 assertEquals(b1, ba);
23485 final Matrix b2 = new Matrix(3, 1);
23486 calibrator.getInitialBiasAsMatrix(b2);
23487 assertEquals(b1, b2);
23488 final Matrix ma1 = calibrator.getInitialMa();
23489 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23490 final Matrix ma2 = new Matrix(3, 3);
23491 calibrator.getInitialMa(ma2);
23492 assertEquals(ma1, ma2);
23493 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23494 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23495 final NEDPosition nedPosition1 = new NEDPosition();
23496 assertTrue(calibrator.getNedPosition(nedPosition1));
23497 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23498 assertNull(calibrator.getMeasurements());
23499 assertTrue(calibrator.isCommonAxisUsed());
23500 assertNull(calibrator.getListener());
23501 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23502 assertFalse(calibrator.isReady());
23503 assertFalse(calibrator.isRunning());
23504 assertNull(calibrator.getEstimatedBiases());
23505 assertFalse(calibrator.getEstimatedBiases(null));
23506 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23507 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23508 assertNull(calibrator.getEstimatedBiasFx());
23509 assertNull(calibrator.getEstimatedBiasFy());
23510 assertNull(calibrator.getEstimatedBiasFz());
23511 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23512 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23513 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23514 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23515 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23516 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23517 assertNull(calibrator.getEstimatedMa());
23518 assertNull(calibrator.getEstimatedSx());
23519 assertNull(calibrator.getEstimatedSy());
23520 assertNull(calibrator.getEstimatedSz());
23521 assertNull(calibrator.getEstimatedMxy());
23522 assertNull(calibrator.getEstimatedMxz());
23523 assertNull(calibrator.getEstimatedMyx());
23524 assertNull(calibrator.getEstimatedMyz());
23525 assertNull(calibrator.getEstimatedMzx());
23526 assertNull(calibrator.getEstimatedMzy());
23527 assertNull(calibrator.getEstimatedCovariance());
23528 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23529 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23530 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23531 assertNotNull(calibrator.getGroundTruthGravityNorm());
23532 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23533 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23534 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23535 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23536 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23537 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23538 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23539 }
23540
23541 @Test
23542 public void testConstructor185() throws WrongSizeException {
23543 final Matrix ba = generateBa();
23544 final double biasX = ba.getElementAtIndex(0);
23545 final double biasY = ba.getElementAtIndex(1);
23546 final double biasZ = ba.getElementAtIndex(2);
23547
23548 final Matrix ma = generateMaCommonAxis();
23549 final double sx = ma.getElementAt(0, 0);
23550 final double sy = ma.getElementAt(1, 1);
23551 final double sz = ma.getElementAt(2, 2);
23552
23553 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23554 final double latitude = Math.toRadians(
23555 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23556 final double longitude = Math.toRadians(
23557 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23558 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23559 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23560 final NEDVelocity nedVelocity = new NEDVelocity();
23561 final ECEFPosition ecefPosition = new ECEFPosition();
23562 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23563 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23564 ecefPosition, ecefVelocity);
23565
23566 final KnownPositionAccelerometerCalibrator calibrator =
23567 new KnownPositionAccelerometerCalibrator(nedPosition,
23568 true, biasX, biasY, biasZ, sx, sy, sz,
23569 this);
23570
23571
23572 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23573 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23574 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23575 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23576 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23577 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23578 final Acceleration bx2 = new Acceleration(0.0,
23579 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23580 calibrator.getInitialBiasXAsAcceleration(bx2);
23581 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23582 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23583 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23584 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23585 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23586 final Acceleration by2 = new Acceleration(0.0,
23587 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23588 calibrator.getInitialBiasYAsAcceleration(by2);
23589 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23590 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23591 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23592 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23593 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23594 final Acceleration bz2 = new Acceleration(0.0,
23595 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23596 calibrator.getInitialBiasZAsAcceleration(bz2);
23597 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23598 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23599 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23600 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23601 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23602 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23603 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23604 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23605 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23606 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23607 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23608 final double[] bias1 = calibrator.getInitialBias();
23609 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23610 final double[] bias2 = new double[3];
23611 calibrator.getInitialBias(bias2);
23612 assertArrayEquals(bias1, bias2, 0.0);
23613 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23614 assertEquals(b1, ba);
23615 final Matrix b2 = new Matrix(3, 1);
23616 calibrator.getInitialBiasAsMatrix(b2);
23617 assertEquals(b1, b2);
23618 final Matrix ma1 = calibrator.getInitialMa();
23619 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23620 final Matrix ma2 = new Matrix(3, 3);
23621 calibrator.getInitialMa(ma2);
23622 assertEquals(ma1, ma2);
23623 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23624 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23625 final NEDPosition nedPosition1 = new NEDPosition();
23626 assertTrue(calibrator.getNedPosition(nedPosition1));
23627 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23628 assertNull(calibrator.getMeasurements());
23629 assertTrue(calibrator.isCommonAxisUsed());
23630 assertSame(calibrator.getListener(), this);
23631 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23632 assertFalse(calibrator.isReady());
23633 assertFalse(calibrator.isRunning());
23634 assertNull(calibrator.getEstimatedBiases());
23635 assertFalse(calibrator.getEstimatedBiases(null));
23636 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23637 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23638 assertNull(calibrator.getEstimatedBiasFx());
23639 assertNull(calibrator.getEstimatedBiasFy());
23640 assertNull(calibrator.getEstimatedBiasFz());
23641 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23642 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23643 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23644 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23645 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23646 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23647 assertNull(calibrator.getEstimatedMa());
23648 assertNull(calibrator.getEstimatedSx());
23649 assertNull(calibrator.getEstimatedSy());
23650 assertNull(calibrator.getEstimatedSz());
23651 assertNull(calibrator.getEstimatedMxy());
23652 assertNull(calibrator.getEstimatedMxz());
23653 assertNull(calibrator.getEstimatedMyx());
23654 assertNull(calibrator.getEstimatedMyz());
23655 assertNull(calibrator.getEstimatedMzx());
23656 assertNull(calibrator.getEstimatedMzy());
23657 assertNull(calibrator.getEstimatedCovariance());
23658 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23659 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23660 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23661 assertNotNull(calibrator.getGroundTruthGravityNorm());
23662 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23663 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23664 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23665 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23666 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23667 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23668 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23669 }
23670
23671 @Test
23672 public void testConstructor186() throws WrongSizeException {
23673 final Collection<StandardDeviationBodyKinematics> measurements =
23674 Collections.emptyList();
23675
23676 final Matrix ba = generateBa();
23677 final double biasX = ba.getElementAtIndex(0);
23678 final double biasY = ba.getElementAtIndex(1);
23679 final double biasZ = ba.getElementAtIndex(2);
23680
23681 final Matrix ma = generateMaCommonAxis();
23682 final double sx = ma.getElementAt(0, 0);
23683 final double sy = ma.getElementAt(1, 1);
23684 final double sz = ma.getElementAt(2, 2);
23685
23686 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23687 final double latitude = Math.toRadians(
23688 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23689 final double longitude = Math.toRadians(
23690 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23691 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23692 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23693 final NEDVelocity nedVelocity = new NEDVelocity();
23694 final ECEFPosition ecefPosition = new ECEFPosition();
23695 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23696 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23697 ecefPosition, ecefVelocity);
23698
23699 final KnownPositionAccelerometerCalibrator calibrator =
23700 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23701 true, biasX, biasY, biasZ, sx, sy, sz);
23702
23703
23704 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23705 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23706 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23707 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23708 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23709 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23710 final Acceleration bx2 = new Acceleration(0.0,
23711 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23712 calibrator.getInitialBiasXAsAcceleration(bx2);
23713 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23714 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23715 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23716 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23717 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23718 final Acceleration by2 = new Acceleration(0.0,
23719 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23720 calibrator.getInitialBiasYAsAcceleration(by2);
23721 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23722 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23723 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23724 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23725 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23726 final Acceleration bz2 = new Acceleration(0.0,
23727 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23728 calibrator.getInitialBiasZAsAcceleration(bz2);
23729 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23730 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23731 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23732 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23733 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23734 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23735 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23736 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23737 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23738 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23739 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23740 final double[] bias1 = calibrator.getInitialBias();
23741 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23742 final double[] bias2 = new double[3];
23743 calibrator.getInitialBias(bias2);
23744 assertArrayEquals(bias1, bias2, 0.0);
23745 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23746 assertEquals(b1, ba);
23747 final Matrix b2 = new Matrix(3, 1);
23748 calibrator.getInitialBiasAsMatrix(b2);
23749 assertEquals(b1, b2);
23750 final Matrix ma1 = calibrator.getInitialMa();
23751 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23752 final Matrix ma2 = new Matrix(3, 3);
23753 calibrator.getInitialMa(ma2);
23754 assertEquals(ma1, ma2);
23755 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23756 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23757 final NEDPosition nedPosition1 = new NEDPosition();
23758 assertTrue(calibrator.getNedPosition(nedPosition1));
23759 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23760 assertSame(calibrator.getMeasurements(), measurements);
23761 assertTrue(calibrator.isCommonAxisUsed());
23762 assertNull(calibrator.getListener());
23763 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23764 assertFalse(calibrator.isReady());
23765 assertFalse(calibrator.isRunning());
23766 assertNull(calibrator.getEstimatedBiases());
23767 assertFalse(calibrator.getEstimatedBiases(null));
23768 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23769 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23770 assertNull(calibrator.getEstimatedBiasFx());
23771 assertNull(calibrator.getEstimatedBiasFy());
23772 assertNull(calibrator.getEstimatedBiasFz());
23773 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23774 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23775 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23776 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23777 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23778 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23779 assertNull(calibrator.getEstimatedMa());
23780 assertNull(calibrator.getEstimatedSx());
23781 assertNull(calibrator.getEstimatedSy());
23782 assertNull(calibrator.getEstimatedSz());
23783 assertNull(calibrator.getEstimatedMxy());
23784 assertNull(calibrator.getEstimatedMxz());
23785 assertNull(calibrator.getEstimatedMyx());
23786 assertNull(calibrator.getEstimatedMyz());
23787 assertNull(calibrator.getEstimatedMzx());
23788 assertNull(calibrator.getEstimatedMzy());
23789 assertNull(calibrator.getEstimatedCovariance());
23790 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23791 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23792 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23793 assertNotNull(calibrator.getGroundTruthGravityNorm());
23794 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23795 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23796 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23797 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23798 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23799 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23800 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23801 }
23802
23803 @Test
23804 public void testConstructor187() throws WrongSizeException {
23805 final Collection<StandardDeviationBodyKinematics> measurements =
23806 Collections.emptyList();
23807
23808 final Matrix ba = generateBa();
23809 final double biasX = ba.getElementAtIndex(0);
23810 final double biasY = ba.getElementAtIndex(1);
23811 final double biasZ = ba.getElementAtIndex(2);
23812
23813 final Matrix ma = generateMaCommonAxis();
23814 final double sx = ma.getElementAt(0, 0);
23815 final double sy = ma.getElementAt(1, 1);
23816 final double sz = ma.getElementAt(2, 2);
23817
23818 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23819 final double latitude = Math.toRadians(
23820 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23821 final double longitude = Math.toRadians(
23822 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23823 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23824 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23825 final NEDVelocity nedVelocity = new NEDVelocity();
23826 final ECEFPosition ecefPosition = new ECEFPosition();
23827 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23828 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23829 ecefPosition, ecefVelocity);
23830
23831 final KnownPositionAccelerometerCalibrator calibrator =
23832 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23833 true, biasX, biasY, biasZ, sx, sy, sz,
23834 this);
23835
23836
23837 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23838 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23839 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23840 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23841 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23842 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23843 final Acceleration bx2 = new Acceleration(0.0,
23844 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23845 calibrator.getInitialBiasXAsAcceleration(bx2);
23846 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23847 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23848 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23849 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23850 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23851 final Acceleration by2 = new Acceleration(0.0,
23852 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23853 calibrator.getInitialBiasYAsAcceleration(by2);
23854 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23855 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23856 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23857 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23858 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23859 final Acceleration bz2 = new Acceleration(0.0,
23860 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23861 calibrator.getInitialBiasZAsAcceleration(bz2);
23862 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23863 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23864 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23865 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23866 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23867 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23868 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23869 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23870 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23871 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23872 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23873 final double[] bias1 = calibrator.getInitialBias();
23874 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23875 final double[] bias2 = new double[3];
23876 calibrator.getInitialBias(bias2);
23877 assertArrayEquals(bias1, bias2, 0.0);
23878 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23879 assertEquals(b1, ba);
23880 final Matrix b2 = new Matrix(3, 1);
23881 calibrator.getInitialBiasAsMatrix(b2);
23882 assertEquals(b1, b2);
23883 final Matrix ma1 = calibrator.getInitialMa();
23884 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23885 final Matrix ma2 = new Matrix(3, 3);
23886 calibrator.getInitialMa(ma2);
23887 assertEquals(ma1, ma2);
23888 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23889 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23890 final NEDPosition nedPosition1 = new NEDPosition();
23891 assertTrue(calibrator.getNedPosition(nedPosition1));
23892 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23893 assertSame(calibrator.getMeasurements(), measurements);
23894 assertTrue(calibrator.isCommonAxisUsed());
23895 assertSame(calibrator.getListener(), this);
23896 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23897 assertFalse(calibrator.isReady());
23898 assertFalse(calibrator.isRunning());
23899 assertNull(calibrator.getEstimatedBiases());
23900 assertFalse(calibrator.getEstimatedBiases(null));
23901 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23902 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23903 assertNull(calibrator.getEstimatedBiasFx());
23904 assertNull(calibrator.getEstimatedBiasFy());
23905 assertNull(calibrator.getEstimatedBiasFz());
23906 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23907 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23908 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23909 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23910 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23911 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23912 assertNull(calibrator.getEstimatedMa());
23913 assertNull(calibrator.getEstimatedSx());
23914 assertNull(calibrator.getEstimatedSy());
23915 assertNull(calibrator.getEstimatedSz());
23916 assertNull(calibrator.getEstimatedMxy());
23917 assertNull(calibrator.getEstimatedMxz());
23918 assertNull(calibrator.getEstimatedMyx());
23919 assertNull(calibrator.getEstimatedMyz());
23920 assertNull(calibrator.getEstimatedMzx());
23921 assertNull(calibrator.getEstimatedMzy());
23922 assertNull(calibrator.getEstimatedCovariance());
23923 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23924 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23925 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23926 assertNotNull(calibrator.getGroundTruthGravityNorm());
23927 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23928 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23929 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23930 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23931 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23932 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23933 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23934 }
23935
23936 @Test
23937 public void testConstructor188() throws WrongSizeException {
23938 final Matrix ba = generateBa();
23939 final double biasX = ba.getElementAtIndex(0);
23940 final double biasY = ba.getElementAtIndex(1);
23941 final double biasZ = ba.getElementAtIndex(2);
23942
23943 final Acceleration bx = new Acceleration(biasX,
23944 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23945 final Acceleration by = new Acceleration(biasY,
23946 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23947 final Acceleration bz = new Acceleration(biasZ,
23948 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23949
23950 final Matrix ma = generateMaCommonAxis();
23951 final double sx = ma.getElementAt(0, 0);
23952 final double sy = ma.getElementAt(1, 1);
23953 final double sz = ma.getElementAt(2, 2);
23954
23955 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23956 final double latitude = Math.toRadians(
23957 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23958 final double longitude = Math.toRadians(
23959 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23960 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23961 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23962 final NEDVelocity nedVelocity = new NEDVelocity();
23963 final ECEFPosition ecefPosition = new ECEFPosition();
23964 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23965 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23966 ecefPosition, ecefVelocity);
23967
23968 final KnownPositionAccelerometerCalibrator calibrator =
23969 new KnownPositionAccelerometerCalibrator(nedPosition,
23970 bx, by, bz, sx, sy, sz);
23971
23972
23973 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23974 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23975 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23976 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23977 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23978 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23979 final Acceleration bx2 = new Acceleration(0.0,
23980 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23981 calibrator.getInitialBiasXAsAcceleration(bx2);
23982 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23983 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23984 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23985 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23986 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23987 final Acceleration by2 = new Acceleration(0.0,
23988 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23989 calibrator.getInitialBiasYAsAcceleration(by2);
23990 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23991 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23992 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23993 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23994 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23995 final Acceleration bz2 = new Acceleration(0.0,
23996 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23997 calibrator.getInitialBiasZAsAcceleration(bz2);
23998 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23999 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24000 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24001 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24002 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24003 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24004 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24005 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24006 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24007 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24008 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24009 final double[] bias1 = calibrator.getInitialBias();
24010 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24011 final double[] bias2 = new double[3];
24012 calibrator.getInitialBias(bias2);
24013 assertArrayEquals(bias1, bias2, 0.0);
24014 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24015 assertEquals(b1, ba);
24016 final Matrix b2 = new Matrix(3, 1);
24017 calibrator.getInitialBiasAsMatrix(b2);
24018 assertEquals(b1, b2);
24019 final Matrix ma1 = calibrator.getInitialMa();
24020 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24021 final Matrix ma2 = new Matrix(3, 3);
24022 calibrator.getInitialMa(ma2);
24023 assertEquals(ma1, ma2);
24024 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24025 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24026 final NEDPosition nedPosition1 = new NEDPosition();
24027 assertTrue(calibrator.getNedPosition(nedPosition1));
24028 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24029 assertNull(calibrator.getMeasurements());
24030 assertFalse(calibrator.isCommonAxisUsed());
24031 assertNull(calibrator.getListener());
24032 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24033 assertFalse(calibrator.isReady());
24034 assertFalse(calibrator.isRunning());
24035 assertNull(calibrator.getEstimatedBiases());
24036 assertFalse(calibrator.getEstimatedBiases(null));
24037 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24038 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24039 assertNull(calibrator.getEstimatedBiasFx());
24040 assertNull(calibrator.getEstimatedBiasFy());
24041 assertNull(calibrator.getEstimatedBiasFz());
24042 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24043 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24044 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24045 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24046 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24047 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24048 assertNull(calibrator.getEstimatedMa());
24049 assertNull(calibrator.getEstimatedSx());
24050 assertNull(calibrator.getEstimatedSy());
24051 assertNull(calibrator.getEstimatedSz());
24052 assertNull(calibrator.getEstimatedMxy());
24053 assertNull(calibrator.getEstimatedMxz());
24054 assertNull(calibrator.getEstimatedMyx());
24055 assertNull(calibrator.getEstimatedMyz());
24056 assertNull(calibrator.getEstimatedMzx());
24057 assertNull(calibrator.getEstimatedMzy());
24058 assertNull(calibrator.getEstimatedCovariance());
24059 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24060 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24061 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24062 assertNotNull(calibrator.getGroundTruthGravityNorm());
24063 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24064 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24065 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24066 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24067 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24068 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24069 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24070 }
24071
24072 @Test
24073 public void testConstructor189() throws WrongSizeException {
24074 final Matrix ba = generateBa();
24075 final double biasX = ba.getElementAtIndex(0);
24076 final double biasY = ba.getElementAtIndex(1);
24077 final double biasZ = ba.getElementAtIndex(2);
24078
24079 final Acceleration bx = new Acceleration(biasX,
24080 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24081 final Acceleration by = new Acceleration(biasY,
24082 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24083 final Acceleration bz = new Acceleration(biasZ,
24084 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24085
24086 final Matrix ma = generateMaCommonAxis();
24087 final double sx = ma.getElementAt(0, 0);
24088 final double sy = ma.getElementAt(1, 1);
24089 final double sz = ma.getElementAt(2, 2);
24090
24091 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24092 final double latitude = Math.toRadians(
24093 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24094 final double longitude = Math.toRadians(
24095 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24096 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24097 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24098 final NEDVelocity nedVelocity = new NEDVelocity();
24099 final ECEFPosition ecefPosition = new ECEFPosition();
24100 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24101 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24102 ecefPosition, ecefVelocity);
24103
24104 final KnownPositionAccelerometerCalibrator calibrator =
24105 new KnownPositionAccelerometerCalibrator(nedPosition,
24106 bx, by, bz, sx, sy, sz, this);
24107
24108
24109 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24110 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24111 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24112 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24113 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24114 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24115 final Acceleration bx2 = new Acceleration(0.0,
24116 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24117 calibrator.getInitialBiasXAsAcceleration(bx2);
24118 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24119 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24120 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24121 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24122 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24123 final Acceleration by2 = new Acceleration(0.0,
24124 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24125 calibrator.getInitialBiasYAsAcceleration(by2);
24126 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24127 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24128 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24129 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24130 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24131 final Acceleration bz2 = new Acceleration(0.0,
24132 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24133 calibrator.getInitialBiasZAsAcceleration(bz2);
24134 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24135 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24136 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24137 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24138 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24139 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24140 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24141 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24142 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24143 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24144 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24145 final double[] bias1 = calibrator.getInitialBias();
24146 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24147 final double[] bias2 = new double[3];
24148 calibrator.getInitialBias(bias2);
24149 assertArrayEquals(bias1, bias2, 0.0);
24150 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24151 assertEquals(b1, ba);
24152 final Matrix b2 = new Matrix(3, 1);
24153 calibrator.getInitialBiasAsMatrix(b2);
24154 assertEquals(b1, b2);
24155 final Matrix ma1 = calibrator.getInitialMa();
24156 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24157 final Matrix ma2 = new Matrix(3, 3);
24158 calibrator.getInitialMa(ma2);
24159 assertEquals(ma1, ma2);
24160 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24161 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24162 final NEDPosition nedPosition1 = new NEDPosition();
24163 assertTrue(calibrator.getNedPosition(nedPosition1));
24164 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24165 assertNull(calibrator.getMeasurements());
24166 assertFalse(calibrator.isCommonAxisUsed());
24167 assertSame(calibrator.getListener(), this);
24168 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24169 assertFalse(calibrator.isReady());
24170 assertFalse(calibrator.isRunning());
24171 assertNull(calibrator.getEstimatedBiases());
24172 assertFalse(calibrator.getEstimatedBiases(null));
24173 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24174 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24175 assertNull(calibrator.getEstimatedBiasFx());
24176 assertNull(calibrator.getEstimatedBiasFy());
24177 assertNull(calibrator.getEstimatedBiasFz());
24178 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24179 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24180 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24181 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24182 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24183 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24184 assertNull(calibrator.getEstimatedMa());
24185 assertNull(calibrator.getEstimatedSx());
24186 assertNull(calibrator.getEstimatedSy());
24187 assertNull(calibrator.getEstimatedSz());
24188 assertNull(calibrator.getEstimatedMxy());
24189 assertNull(calibrator.getEstimatedMxz());
24190 assertNull(calibrator.getEstimatedMyx());
24191 assertNull(calibrator.getEstimatedMyz());
24192 assertNull(calibrator.getEstimatedMzx());
24193 assertNull(calibrator.getEstimatedMzy());
24194 assertNull(calibrator.getEstimatedCovariance());
24195 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24196 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24197 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24198 assertNotNull(calibrator.getGroundTruthGravityNorm());
24199 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24200 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24201 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24202 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24203 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24204 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24205 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24206 }
24207
24208 @Test
24209 public void testConstructor190() throws WrongSizeException {
24210 final Collection<StandardDeviationBodyKinematics> measurements =
24211 Collections.emptyList();
24212
24213 final Matrix ba = generateBa();
24214 final double biasX = ba.getElementAtIndex(0);
24215 final double biasY = ba.getElementAtIndex(1);
24216 final double biasZ = ba.getElementAtIndex(2);
24217
24218 final Acceleration bx = new Acceleration(biasX,
24219 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24220 final Acceleration by = new Acceleration(biasY,
24221 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24222 final Acceleration bz = new Acceleration(biasZ,
24223 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24224
24225 final Matrix ma = generateMaCommonAxis();
24226 final double sx = ma.getElementAt(0, 0);
24227 final double sy = ma.getElementAt(1, 1);
24228 final double sz = ma.getElementAt(2, 2);
24229
24230 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24231 final double latitude = Math.toRadians(
24232 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24233 final double longitude = Math.toRadians(
24234 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24235 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24236 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24237 final NEDVelocity nedVelocity = new NEDVelocity();
24238 final ECEFPosition ecefPosition = new ECEFPosition();
24239 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24240 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24241 ecefPosition, ecefVelocity);
24242
24243 final KnownPositionAccelerometerCalibrator calibrator =
24244 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24245 bx, by, bz, sx, sy, sz);
24246
24247
24248 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24249 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24250 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24251 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24252 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24253 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24254 final Acceleration bx2 = new Acceleration(0.0,
24255 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24256 calibrator.getInitialBiasXAsAcceleration(bx2);
24257 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24258 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24259 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24260 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24261 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24262 final Acceleration by2 = new Acceleration(0.0,
24263 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24264 calibrator.getInitialBiasYAsAcceleration(by2);
24265 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24266 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24267 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24268 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24269 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24270 final Acceleration bz2 = new Acceleration(0.0,
24271 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24272 calibrator.getInitialBiasZAsAcceleration(bz2);
24273 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24274 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24275 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24276 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24277 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24278 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24279 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24280 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24281 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24282 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24283 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24284 final double[] bias1 = calibrator.getInitialBias();
24285 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24286 final double[] bias2 = new double[3];
24287 calibrator.getInitialBias(bias2);
24288 assertArrayEquals(bias1, bias2, 0.0);
24289 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24290 assertEquals(b1, ba);
24291 final Matrix b2 = new Matrix(3, 1);
24292 calibrator.getInitialBiasAsMatrix(b2);
24293 assertEquals(b1, b2);
24294 final Matrix ma1 = calibrator.getInitialMa();
24295 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24296 final Matrix ma2 = new Matrix(3, 3);
24297 calibrator.getInitialMa(ma2);
24298 assertEquals(ma1, ma2);
24299 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24300 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24301 final NEDPosition nedPosition1 = new NEDPosition();
24302 assertTrue(calibrator.getNedPosition(nedPosition1));
24303 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24304 assertSame(calibrator.getMeasurements(), measurements);
24305 assertFalse(calibrator.isCommonAxisUsed());
24306 assertNull(calibrator.getListener());
24307 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24308 assertFalse(calibrator.isReady());
24309 assertFalse(calibrator.isRunning());
24310 assertNull(calibrator.getEstimatedBiases());
24311 assertFalse(calibrator.getEstimatedBiases(null));
24312 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24313 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24314 assertNull(calibrator.getEstimatedBiasFx());
24315 assertNull(calibrator.getEstimatedBiasFy());
24316 assertNull(calibrator.getEstimatedBiasFz());
24317 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24318 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24319 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24320 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24321 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24322 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24323 assertNull(calibrator.getEstimatedMa());
24324 assertNull(calibrator.getEstimatedSx());
24325 assertNull(calibrator.getEstimatedSy());
24326 assertNull(calibrator.getEstimatedSz());
24327 assertNull(calibrator.getEstimatedMxy());
24328 assertNull(calibrator.getEstimatedMxz());
24329 assertNull(calibrator.getEstimatedMyx());
24330 assertNull(calibrator.getEstimatedMyz());
24331 assertNull(calibrator.getEstimatedMzx());
24332 assertNull(calibrator.getEstimatedMzy());
24333 assertNull(calibrator.getEstimatedCovariance());
24334 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24335 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24336 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24337 assertNotNull(calibrator.getGroundTruthGravityNorm());
24338 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24339 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24340 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24341 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24342 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24343 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24344 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24345 }
24346
24347 @Test
24348 public void testConstructor191() throws WrongSizeException {
24349 final Collection<StandardDeviationBodyKinematics> measurements =
24350 Collections.emptyList();
24351
24352 final Matrix ba = generateBa();
24353 final double biasX = ba.getElementAtIndex(0);
24354 final double biasY = ba.getElementAtIndex(1);
24355 final double biasZ = ba.getElementAtIndex(2);
24356
24357 final Acceleration bx = new Acceleration(biasX,
24358 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24359 final Acceleration by = new Acceleration(biasY,
24360 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24361 final Acceleration bz = new Acceleration(biasZ,
24362 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24363
24364 final Matrix ma = generateMaCommonAxis();
24365 final double sx = ma.getElementAt(0, 0);
24366 final double sy = ma.getElementAt(1, 1);
24367 final double sz = ma.getElementAt(2, 2);
24368
24369 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24370 final double latitude = Math.toRadians(
24371 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24372 final double longitude = Math.toRadians(
24373 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24374 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24375 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24376 final NEDVelocity nedVelocity = new NEDVelocity();
24377 final ECEFPosition ecefPosition = new ECEFPosition();
24378 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24379 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24380 ecefPosition, ecefVelocity);
24381
24382 final KnownPositionAccelerometerCalibrator calibrator =
24383 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24384 bx, by, bz, sx, sy, sz, this);
24385
24386
24387 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24388 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24389 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24390 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24391 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24392 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24393 final Acceleration bx2 = new Acceleration(0.0,
24394 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24395 calibrator.getInitialBiasXAsAcceleration(bx2);
24396 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24397 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24398 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24399 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24400 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24401 final Acceleration by2 = new Acceleration(0.0,
24402 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24403 calibrator.getInitialBiasYAsAcceleration(by2);
24404 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24405 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24406 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24407 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24408 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24409 final Acceleration bz2 = new Acceleration(0.0,
24410 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24411 calibrator.getInitialBiasZAsAcceleration(bz2);
24412 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24413 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24414 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24415 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24416 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24417 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24418 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24419 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24420 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24421 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24422 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24423 final double[] bias1 = calibrator.getInitialBias();
24424 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24425 final double[] bias2 = new double[3];
24426 calibrator.getInitialBias(bias2);
24427 assertArrayEquals(bias1, bias2, 0.0);
24428 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24429 assertEquals(b1, ba);
24430 final Matrix b2 = new Matrix(3, 1);
24431 calibrator.getInitialBiasAsMatrix(b2);
24432 assertEquals(b1, b2);
24433 final Matrix ma1 = calibrator.getInitialMa();
24434 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24435 final Matrix ma2 = new Matrix(3, 3);
24436 calibrator.getInitialMa(ma2);
24437 assertEquals(ma1, ma2);
24438 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24439 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24440 final NEDPosition nedPosition1 = new NEDPosition();
24441 assertTrue(calibrator.getNedPosition(nedPosition1));
24442 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24443 assertSame(calibrator.getMeasurements(), measurements);
24444 assertFalse(calibrator.isCommonAxisUsed());
24445 assertSame(calibrator.getListener(), this);
24446 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24447 assertFalse(calibrator.isReady());
24448 assertFalse(calibrator.isRunning());
24449 assertNull(calibrator.getEstimatedBiases());
24450 assertFalse(calibrator.getEstimatedBiases(null));
24451 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24452 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24453 assertNull(calibrator.getEstimatedBiasFx());
24454 assertNull(calibrator.getEstimatedBiasFy());
24455 assertNull(calibrator.getEstimatedBiasFz());
24456 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24457 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24458 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24459 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24460 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24461 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24462 assertNull(calibrator.getEstimatedMa());
24463 assertNull(calibrator.getEstimatedSx());
24464 assertNull(calibrator.getEstimatedSy());
24465 assertNull(calibrator.getEstimatedSz());
24466 assertNull(calibrator.getEstimatedMxy());
24467 assertNull(calibrator.getEstimatedMxz());
24468 assertNull(calibrator.getEstimatedMyx());
24469 assertNull(calibrator.getEstimatedMyz());
24470 assertNull(calibrator.getEstimatedMzx());
24471 assertNull(calibrator.getEstimatedMzy());
24472 assertNull(calibrator.getEstimatedCovariance());
24473 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24474 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24475 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24476 assertNotNull(calibrator.getGroundTruthGravityNorm());
24477 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24478 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24479 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24480 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24481 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24482 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24483 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24484 }
24485
24486 @Test
24487 public void testConstructor192() throws WrongSizeException {
24488 final Matrix ba = generateBa();
24489 final double biasX = ba.getElementAtIndex(0);
24490 final double biasY = ba.getElementAtIndex(1);
24491 final double biasZ = ba.getElementAtIndex(2);
24492
24493 final Acceleration bx = new Acceleration(biasX,
24494 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24495 final Acceleration by = new Acceleration(biasY,
24496 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24497 final Acceleration bz = new Acceleration(biasZ,
24498 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24499
24500 final Matrix ma = generateMaCommonAxis();
24501 final double sx = ma.getElementAt(0, 0);
24502 final double sy = ma.getElementAt(1, 1);
24503 final double sz = ma.getElementAt(2, 2);
24504
24505 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24506 final double latitude = Math.toRadians(
24507 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24508 final double longitude = Math.toRadians(
24509 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24510 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24511 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24512 final NEDVelocity nedVelocity = new NEDVelocity();
24513 final ECEFPosition ecefPosition = new ECEFPosition();
24514 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24515 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24516 ecefPosition, ecefVelocity);
24517
24518 final KnownPositionAccelerometerCalibrator calibrator =
24519 new KnownPositionAccelerometerCalibrator(nedPosition,
24520 true, bx, by, bz, sx, sy, sz);
24521
24522
24523 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24524 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24525 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24526 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24527 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24528 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24529 final Acceleration bx2 = new Acceleration(0.0,
24530 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24531 calibrator.getInitialBiasXAsAcceleration(bx2);
24532 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24533 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24534 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24535 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24536 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24537 final Acceleration by2 = new Acceleration(0.0,
24538 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24539 calibrator.getInitialBiasYAsAcceleration(by2);
24540 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24541 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24542 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24543 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24544 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24545 final Acceleration bz2 = new Acceleration(0.0,
24546 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24547 calibrator.getInitialBiasZAsAcceleration(bz2);
24548 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24549 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24550 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24551 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24552 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24553 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24554 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24555 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24556 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24557 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24558 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24559 final double[] bias1 = calibrator.getInitialBias();
24560 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24561 final double[] bias2 = new double[3];
24562 calibrator.getInitialBias(bias2);
24563 assertArrayEquals(bias1, bias2, 0.0);
24564 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24565 assertEquals(b1, ba);
24566 final Matrix b2 = new Matrix(3, 1);
24567 calibrator.getInitialBiasAsMatrix(b2);
24568 assertEquals(b1, b2);
24569 final Matrix ma1 = calibrator.getInitialMa();
24570 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24571 final Matrix ma2 = new Matrix(3, 3);
24572 calibrator.getInitialMa(ma2);
24573 assertEquals(ma1, ma2);
24574 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24575 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24576 final NEDPosition nedPosition1 = new NEDPosition();
24577 assertTrue(calibrator.getNedPosition(nedPosition1));
24578 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24579 assertNull(calibrator.getMeasurements());
24580 assertTrue(calibrator.isCommonAxisUsed());
24581 assertNull(calibrator.getListener());
24582 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24583 assertFalse(calibrator.isReady());
24584 assertFalse(calibrator.isRunning());
24585 assertNull(calibrator.getEstimatedBiases());
24586 assertFalse(calibrator.getEstimatedBiases(null));
24587 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24588 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24589 assertNull(calibrator.getEstimatedBiasFx());
24590 assertNull(calibrator.getEstimatedBiasFy());
24591 assertNull(calibrator.getEstimatedBiasFz());
24592 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24593 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24594 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24595 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24596 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24597 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24598 assertNull(calibrator.getEstimatedMa());
24599 assertNull(calibrator.getEstimatedSx());
24600 assertNull(calibrator.getEstimatedSy());
24601 assertNull(calibrator.getEstimatedSz());
24602 assertNull(calibrator.getEstimatedMxy());
24603 assertNull(calibrator.getEstimatedMxz());
24604 assertNull(calibrator.getEstimatedMyx());
24605 assertNull(calibrator.getEstimatedMyz());
24606 assertNull(calibrator.getEstimatedMzx());
24607 assertNull(calibrator.getEstimatedMzy());
24608 assertNull(calibrator.getEstimatedCovariance());
24609 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24610 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24611 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24612 assertNotNull(calibrator.getGroundTruthGravityNorm());
24613 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24614 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24615 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24616 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24617 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24618 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24619 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24620 }
24621
24622 @Test
24623 public void testConstructor193() throws WrongSizeException {
24624 final Matrix ba = generateBa();
24625 final double biasX = ba.getElementAtIndex(0);
24626 final double biasY = ba.getElementAtIndex(1);
24627 final double biasZ = ba.getElementAtIndex(2);
24628
24629 final Acceleration bx = new Acceleration(biasX,
24630 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24631 final Acceleration by = new Acceleration(biasY,
24632 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24633 final Acceleration bz = new Acceleration(biasZ,
24634 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24635
24636 final Matrix ma = generateMaCommonAxis();
24637 final double sx = ma.getElementAt(0, 0);
24638 final double sy = ma.getElementAt(1, 1);
24639 final double sz = ma.getElementAt(2, 2);
24640
24641 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24642 final double latitude = Math.toRadians(
24643 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24644 final double longitude = Math.toRadians(
24645 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24646 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24647 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24648 final NEDVelocity nedVelocity = new NEDVelocity();
24649 final ECEFPosition ecefPosition = new ECEFPosition();
24650 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24651 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24652 ecefPosition, ecefVelocity);
24653
24654 final KnownPositionAccelerometerCalibrator calibrator =
24655 new KnownPositionAccelerometerCalibrator(nedPosition,
24656 true, bx, by, bz, sx, sy, sz, this);
24657
24658
24659 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24660 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24661 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24662 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24663 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24664 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24665 final Acceleration bx2 = new Acceleration(0.0,
24666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24667 calibrator.getInitialBiasXAsAcceleration(bx2);
24668 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24669 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24670 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24671 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24672 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24673 final Acceleration by2 = new Acceleration(0.0,
24674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24675 calibrator.getInitialBiasYAsAcceleration(by2);
24676 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24677 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24678 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24679 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24680 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24681 final Acceleration bz2 = new Acceleration(0.0,
24682 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24683 calibrator.getInitialBiasZAsAcceleration(bz2);
24684 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24685 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24686 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24687 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24688 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24689 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24690 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24691 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24692 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24693 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24694 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24695 final double[] bias1 = calibrator.getInitialBias();
24696 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24697 final double[] bias2 = new double[3];
24698 calibrator.getInitialBias(bias2);
24699 assertArrayEquals(bias1, bias2, 0.0);
24700 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24701 assertEquals(b1, ba);
24702 final Matrix b2 = new Matrix(3, 1);
24703 calibrator.getInitialBiasAsMatrix(b2);
24704 assertEquals(b1, b2);
24705 final Matrix ma1 = calibrator.getInitialMa();
24706 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24707 final Matrix ma2 = new Matrix(3, 3);
24708 calibrator.getInitialMa(ma2);
24709 assertEquals(ma1, ma2);
24710 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24711 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24712 final NEDPosition nedPosition1 = new NEDPosition();
24713 assertTrue(calibrator.getNedPosition(nedPosition1));
24714 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24715 assertNull(calibrator.getMeasurements());
24716 assertTrue(calibrator.isCommonAxisUsed());
24717 assertSame(calibrator.getListener(), this);
24718 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24719 assertFalse(calibrator.isReady());
24720 assertFalse(calibrator.isRunning());
24721 assertNull(calibrator.getEstimatedBiases());
24722 assertFalse(calibrator.getEstimatedBiases(null));
24723 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24724 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24725 assertNull(calibrator.getEstimatedBiasFx());
24726 assertNull(calibrator.getEstimatedBiasFy());
24727 assertNull(calibrator.getEstimatedBiasFz());
24728 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24729 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24730 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24731 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24732 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24733 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24734 assertNull(calibrator.getEstimatedMa());
24735 assertNull(calibrator.getEstimatedSx());
24736 assertNull(calibrator.getEstimatedSy());
24737 assertNull(calibrator.getEstimatedSz());
24738 assertNull(calibrator.getEstimatedMxy());
24739 assertNull(calibrator.getEstimatedMxz());
24740 assertNull(calibrator.getEstimatedMyx());
24741 assertNull(calibrator.getEstimatedMyz());
24742 assertNull(calibrator.getEstimatedMzx());
24743 assertNull(calibrator.getEstimatedMzy());
24744 assertNull(calibrator.getEstimatedCovariance());
24745 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24746 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24747 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24748 assertNotNull(calibrator.getGroundTruthGravityNorm());
24749 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24750 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24751 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24752 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24753 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24754 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24755 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24756 }
24757
24758 @Test
24759 public void testConstructor194() throws WrongSizeException {
24760 final Collection<StandardDeviationBodyKinematics> measurements =
24761 Collections.emptyList();
24762
24763 final Matrix ba = generateBa();
24764 final double biasX = ba.getElementAtIndex(0);
24765 final double biasY = ba.getElementAtIndex(1);
24766 final double biasZ = ba.getElementAtIndex(2);
24767
24768 final Acceleration bx = new Acceleration(biasX,
24769 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24770 final Acceleration by = new Acceleration(biasY,
24771 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24772 final Acceleration bz = new Acceleration(biasZ,
24773 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24774
24775 final Matrix ma = generateMaCommonAxis();
24776 final double sx = ma.getElementAt(0, 0);
24777 final double sy = ma.getElementAt(1, 1);
24778 final double sz = ma.getElementAt(2, 2);
24779
24780 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24781 final double latitude = Math.toRadians(
24782 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24783 final double longitude = Math.toRadians(
24784 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24785 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24786 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24787 final NEDVelocity nedVelocity = new NEDVelocity();
24788 final ECEFPosition ecefPosition = new ECEFPosition();
24789 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24790 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24791 ecefPosition, ecefVelocity);
24792
24793 final KnownPositionAccelerometerCalibrator calibrator =
24794 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24795 true, bx, by, bz, sx, sy, sz);
24796
24797
24798 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24799 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24800 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24801 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24802 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24803 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24804 final Acceleration bx2 = new Acceleration(0.0,
24805 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24806 calibrator.getInitialBiasXAsAcceleration(bx2);
24807 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24808 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24809 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24810 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24811 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24812 final Acceleration by2 = new Acceleration(0.0,
24813 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24814 calibrator.getInitialBiasYAsAcceleration(by2);
24815 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24816 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24817 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24818 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24819 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24820 final Acceleration bz2 = new Acceleration(0.0,
24821 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24822 calibrator.getInitialBiasZAsAcceleration(bz2);
24823 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24824 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24825 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24826 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24827 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24828 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24829 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24830 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24831 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24832 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24833 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24834 final double[] bias1 = calibrator.getInitialBias();
24835 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24836 final double[] bias2 = new double[3];
24837 calibrator.getInitialBias(bias2);
24838 assertArrayEquals(bias1, bias2, 0.0);
24839 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24840 assertEquals(b1, ba);
24841 final Matrix b2 = new Matrix(3, 1);
24842 calibrator.getInitialBiasAsMatrix(b2);
24843 assertEquals(b1, b2);
24844 final Matrix ma1 = calibrator.getInitialMa();
24845 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24846 final Matrix ma2 = new Matrix(3, 3);
24847 calibrator.getInitialMa(ma2);
24848 assertEquals(ma1, ma2);
24849 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24850 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24851 final NEDPosition nedPosition1 = new NEDPosition();
24852 assertTrue(calibrator.getNedPosition(nedPosition1));
24853 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24854 assertSame(calibrator.getMeasurements(), measurements);
24855 assertTrue(calibrator.isCommonAxisUsed());
24856 assertNull(calibrator.getListener());
24857 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24858 assertFalse(calibrator.isReady());
24859 assertFalse(calibrator.isRunning());
24860 assertNull(calibrator.getEstimatedBiases());
24861 assertFalse(calibrator.getEstimatedBiases(null));
24862 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24863 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24864 assertNull(calibrator.getEstimatedBiasFx());
24865 assertNull(calibrator.getEstimatedBiasFy());
24866 assertNull(calibrator.getEstimatedBiasFz());
24867 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24868 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24869 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24870 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24871 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24872 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24873 assertNull(calibrator.getEstimatedMa());
24874 assertNull(calibrator.getEstimatedSx());
24875 assertNull(calibrator.getEstimatedSy());
24876 assertNull(calibrator.getEstimatedSz());
24877 assertNull(calibrator.getEstimatedMxy());
24878 assertNull(calibrator.getEstimatedMxz());
24879 assertNull(calibrator.getEstimatedMyx());
24880 assertNull(calibrator.getEstimatedMyz());
24881 assertNull(calibrator.getEstimatedMzx());
24882 assertNull(calibrator.getEstimatedMzy());
24883 assertNull(calibrator.getEstimatedCovariance());
24884 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24885 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24886 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24887 assertNotNull(calibrator.getGroundTruthGravityNorm());
24888 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24889 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24890 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24891 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24892 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24893 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24894 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24895 }
24896
24897 @Test
24898 public void testConstructor195() throws WrongSizeException {
24899 final Collection<StandardDeviationBodyKinematics> measurements =
24900 Collections.emptyList();
24901
24902 final Matrix ba = generateBa();
24903 final double biasX = ba.getElementAtIndex(0);
24904 final double biasY = ba.getElementAtIndex(1);
24905 final double biasZ = ba.getElementAtIndex(2);
24906
24907 final Acceleration bx = new Acceleration(biasX,
24908 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24909 final Acceleration by = new Acceleration(biasY,
24910 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24911 final Acceleration bz = new Acceleration(biasZ,
24912 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24913
24914 final Matrix ma = generateMaCommonAxis();
24915 final double sx = ma.getElementAt(0, 0);
24916 final double sy = ma.getElementAt(1, 1);
24917 final double sz = ma.getElementAt(2, 2);
24918
24919 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24920 final double latitude = Math.toRadians(
24921 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24922 final double longitude = Math.toRadians(
24923 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24924 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24925 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24926 final NEDVelocity nedVelocity = new NEDVelocity();
24927 final ECEFPosition ecefPosition = new ECEFPosition();
24928 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24929 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24930 ecefPosition, ecefVelocity);
24931
24932 final KnownPositionAccelerometerCalibrator calibrator =
24933 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24934 true, bx, by, bz, sx, sy, sz, this);
24935
24936
24937 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24938 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24939 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24940 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24941 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24942 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24943 final Acceleration bx2 = new Acceleration(0.0,
24944 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24945 calibrator.getInitialBiasXAsAcceleration(bx2);
24946 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24947 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24948 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24949 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24950 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24951 final Acceleration by2 = new Acceleration(0.0,
24952 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24953 calibrator.getInitialBiasYAsAcceleration(by2);
24954 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24955 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24956 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24957 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24958 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24959 final Acceleration bz2 = new Acceleration(0.0,
24960 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24961 calibrator.getInitialBiasZAsAcceleration(bz2);
24962 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24963 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24964 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24965 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24966 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24967 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24968 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24969 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24970 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24971 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24972 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24973 final double[] bias1 = calibrator.getInitialBias();
24974 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24975 final double[] bias2 = new double[3];
24976 calibrator.getInitialBias(bias2);
24977 assertArrayEquals(bias1, bias2, 0.0);
24978 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24979 assertEquals(b1, ba);
24980 final Matrix b2 = new Matrix(3, 1);
24981 calibrator.getInitialBiasAsMatrix(b2);
24982 assertEquals(b1, b2);
24983 final Matrix ma1 = calibrator.getInitialMa();
24984 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24985 final Matrix ma2 = new Matrix(3, 3);
24986 calibrator.getInitialMa(ma2);
24987 assertEquals(ma1, ma2);
24988 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24989 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24990 final NEDPosition nedPosition1 = new NEDPosition();
24991 assertTrue(calibrator.getNedPosition(nedPosition1));
24992 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24993 assertSame(calibrator.getMeasurements(), measurements);
24994 assertTrue(calibrator.isCommonAxisUsed());
24995 assertSame(calibrator.getListener(), this);
24996 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24997 assertFalse(calibrator.isReady());
24998 assertFalse(calibrator.isRunning());
24999 assertNull(calibrator.getEstimatedBiases());
25000 assertFalse(calibrator.getEstimatedBiases(null));
25001 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25002 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25003 assertNull(calibrator.getEstimatedBiasFx());
25004 assertNull(calibrator.getEstimatedBiasFy());
25005 assertNull(calibrator.getEstimatedBiasFz());
25006 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25007 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25008 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25009 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25010 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25011 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25012 assertNull(calibrator.getEstimatedMa());
25013 assertNull(calibrator.getEstimatedSx());
25014 assertNull(calibrator.getEstimatedSy());
25015 assertNull(calibrator.getEstimatedSz());
25016 assertNull(calibrator.getEstimatedMxy());
25017 assertNull(calibrator.getEstimatedMxz());
25018 assertNull(calibrator.getEstimatedMyx());
25019 assertNull(calibrator.getEstimatedMyz());
25020 assertNull(calibrator.getEstimatedMzx());
25021 assertNull(calibrator.getEstimatedMzy());
25022 assertNull(calibrator.getEstimatedCovariance());
25023 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25024 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25025 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25026 assertNotNull(calibrator.getGroundTruthGravityNorm());
25027 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25028 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25029 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25030 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25031 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25032 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25033 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25034 }
25035
25036 @Test
25037 public void testConstructor196() throws WrongSizeException {
25038 final Matrix ba = generateBa();
25039 final double biasX = ba.getElementAtIndex(0);
25040 final double biasY = ba.getElementAtIndex(1);
25041 final double biasZ = ba.getElementAtIndex(2);
25042
25043 final Matrix ma = generateMaCommonAxis();
25044 final double sx = ma.getElementAt(0, 0);
25045 final double sy = ma.getElementAt(1, 1);
25046 final double sz = ma.getElementAt(2, 2);
25047 final double mxy = ma.getElementAt(0, 1);
25048 final double mxz = ma.getElementAt(0, 2);
25049 final double myx = ma.getElementAt(1, 0);
25050 final double myz = ma.getElementAt(1, 2);
25051 final double mzx = ma.getElementAt(2, 0);
25052 final double mzy = ma.getElementAt(2, 1);
25053
25054 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25055 final double latitude = Math.toRadians(
25056 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25057 final double longitude = Math.toRadians(
25058 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25059 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25060 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25061 final NEDVelocity nedVelocity = new NEDVelocity();
25062 final ECEFPosition ecefPosition = new ECEFPosition();
25063 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25064 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25065 ecefPosition, ecefVelocity);
25066
25067 final KnownPositionAccelerometerCalibrator calibrator =
25068 new KnownPositionAccelerometerCalibrator(nedPosition,
25069 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25070 myx, myz, mzx, mzy);
25071
25072
25073 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25074 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25075 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25076 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25077 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25078 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25079 final Acceleration bx2 = new Acceleration(0.0,
25080 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25081 calibrator.getInitialBiasXAsAcceleration(bx2);
25082 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25083 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25084 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25085 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25086 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25087 final Acceleration by2 = new Acceleration(0.0,
25088 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25089 calibrator.getInitialBiasYAsAcceleration(by2);
25090 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25091 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25092 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25093 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25094 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25095 final Acceleration bz2 = new Acceleration(0.0,
25096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25097 calibrator.getInitialBiasZAsAcceleration(bz2);
25098 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25099 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25100 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25101 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25102 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25103 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25104 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25105 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25106 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25107 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25108 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25109 final double[] bias1 = calibrator.getInitialBias();
25110 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25111 final double[] bias2 = new double[3];
25112 calibrator.getInitialBias(bias2);
25113 assertArrayEquals(bias1, bias2, 0.0);
25114 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25115 assertEquals(b1, ba);
25116 final Matrix b2 = new Matrix(3, 1);
25117 calibrator.getInitialBiasAsMatrix(b2);
25118 assertEquals(b1, b2);
25119 final Matrix ma1 = new Matrix(3, 3);
25120 ma1.setSubmatrix(0, 0,
25121 2, 2,
25122 new double[]{sx, myx, mzx,
25123 mxy, sy, mzy,
25124 mxz, myz, sz});
25125 assertEquals(calibrator.getInitialMa(), ma1);
25126 final Matrix ma2 = new Matrix(3, 3);
25127 calibrator.getInitialMa(ma2);
25128 assertEquals(ma1, ma2);
25129 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25130 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25131 final NEDPosition nedPosition1 = new NEDPosition();
25132 assertTrue(calibrator.getNedPosition(nedPosition1));
25133 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25134 assertNull(calibrator.getMeasurements());
25135 assertFalse(calibrator.isCommonAxisUsed());
25136 assertNull(calibrator.getListener());
25137 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25138 assertFalse(calibrator.isReady());
25139 assertFalse(calibrator.isRunning());
25140 assertNull(calibrator.getEstimatedBiases());
25141 assertFalse(calibrator.getEstimatedBiases(null));
25142 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25143 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25144 assertNull(calibrator.getEstimatedBiasFx());
25145 assertNull(calibrator.getEstimatedBiasFy());
25146 assertNull(calibrator.getEstimatedBiasFz());
25147 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25148 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25149 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25150 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25151 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25152 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25153 assertNull(calibrator.getEstimatedMa());
25154 assertNull(calibrator.getEstimatedSx());
25155 assertNull(calibrator.getEstimatedSy());
25156 assertNull(calibrator.getEstimatedSz());
25157 assertNull(calibrator.getEstimatedMxy());
25158 assertNull(calibrator.getEstimatedMxz());
25159 assertNull(calibrator.getEstimatedMyx());
25160 assertNull(calibrator.getEstimatedMyz());
25161 assertNull(calibrator.getEstimatedMzx());
25162 assertNull(calibrator.getEstimatedMzy());
25163 assertNull(calibrator.getEstimatedCovariance());
25164 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25165 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25166 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25167 assertNotNull(calibrator.getGroundTruthGravityNorm());
25168 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25169 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25170 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25171 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25172 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25173 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25174 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25175 }
25176
25177 @Test
25178 public void testConstructor197() throws WrongSizeException {
25179 final Collection<StandardDeviationBodyKinematics> measurements =
25180 Collections.emptyList();
25181
25182 final Matrix ba = generateBa();
25183 final double biasX = ba.getElementAtIndex(0);
25184 final double biasY = ba.getElementAtIndex(1);
25185 final double biasZ = ba.getElementAtIndex(2);
25186
25187 final Matrix ma = generateMaCommonAxis();
25188 final double sx = ma.getElementAt(0, 0);
25189 final double sy = ma.getElementAt(1, 1);
25190 final double sz = ma.getElementAt(2, 2);
25191 final double mxy = ma.getElementAt(0, 1);
25192 final double mxz = ma.getElementAt(0, 2);
25193 final double myx = ma.getElementAt(1, 0);
25194 final double myz = ma.getElementAt(1, 2);
25195 final double mzx = ma.getElementAt(2, 0);
25196 final double mzy = ma.getElementAt(2, 1);
25197
25198 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25199 final double latitude = Math.toRadians(
25200 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25201 final double longitude = Math.toRadians(
25202 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25203 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25204 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25205 final NEDVelocity nedVelocity = new NEDVelocity();
25206 final ECEFPosition ecefPosition = new ECEFPosition();
25207 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25208 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25209 ecefPosition, ecefVelocity);
25210
25211 final KnownPositionAccelerometerCalibrator calibrator =
25212 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25213 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25214 myx, myz, mzx, mzy);
25215
25216
25217 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25218 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25219 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25220 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25221 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25222 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25223 final Acceleration bx2 = new Acceleration(0.0,
25224 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25225 calibrator.getInitialBiasXAsAcceleration(bx2);
25226 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25227 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25228 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25229 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25230 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25231 final Acceleration by2 = new Acceleration(0.0,
25232 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25233 calibrator.getInitialBiasYAsAcceleration(by2);
25234 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25235 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25236 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25237 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25238 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25239 final Acceleration bz2 = new Acceleration(0.0,
25240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25241 calibrator.getInitialBiasZAsAcceleration(bz2);
25242 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25243 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25244 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25245 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25246 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25247 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25248 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25249 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25250 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25251 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25252 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25253 final double[] bias1 = calibrator.getInitialBias();
25254 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25255 final double[] bias2 = new double[3];
25256 calibrator.getInitialBias(bias2);
25257 assertArrayEquals(bias1, bias2, 0.0);
25258 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25259 assertEquals(b1, ba);
25260 final Matrix b2 = new Matrix(3, 1);
25261 calibrator.getInitialBiasAsMatrix(b2);
25262 assertEquals(b1, b2);
25263 final Matrix ma1 = new Matrix(3, 3);
25264 ma1.setSubmatrix(0, 0,
25265 2, 2,
25266 new double[]{sx, myx, mzx,
25267 mxy, sy, mzy,
25268 mxz, myz, sz});
25269 assertEquals(calibrator.getInitialMa(), ma1);
25270 final Matrix ma2 = new Matrix(3, 3);
25271 calibrator.getInitialMa(ma2);
25272 assertEquals(ma1, ma2);
25273 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25274 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25275 final NEDPosition nedPosition1 = new NEDPosition();
25276 assertTrue(calibrator.getNedPosition(nedPosition1));
25277 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25278 assertSame(calibrator.getMeasurements(), measurements);
25279 assertFalse(calibrator.isCommonAxisUsed());
25280 assertNull(calibrator.getListener());
25281 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25282 assertFalse(calibrator.isReady());
25283 assertFalse(calibrator.isRunning());
25284 assertNull(calibrator.getEstimatedBiases());
25285 assertFalse(calibrator.getEstimatedBiases(null));
25286 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25287 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25288 assertNull(calibrator.getEstimatedBiasFx());
25289 assertNull(calibrator.getEstimatedBiasFy());
25290 assertNull(calibrator.getEstimatedBiasFz());
25291 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25292 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25293 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25294 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25295 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25296 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25297 assertNull(calibrator.getEstimatedMa());
25298 assertNull(calibrator.getEstimatedSx());
25299 assertNull(calibrator.getEstimatedSy());
25300 assertNull(calibrator.getEstimatedSz());
25301 assertNull(calibrator.getEstimatedMxy());
25302 assertNull(calibrator.getEstimatedMxz());
25303 assertNull(calibrator.getEstimatedMyx());
25304 assertNull(calibrator.getEstimatedMyz());
25305 assertNull(calibrator.getEstimatedMzx());
25306 assertNull(calibrator.getEstimatedMzy());
25307 assertNull(calibrator.getEstimatedCovariance());
25308 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25309 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25310 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25311 assertNotNull(calibrator.getGroundTruthGravityNorm());
25312 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25313 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25314 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25315 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25316 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25317 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25318 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25319 }
25320
25321 @Test
25322 public void testConstructor198() throws WrongSizeException {
25323 final Collection<StandardDeviationBodyKinematics> measurements =
25324 Collections.emptyList();
25325
25326 final Matrix ba = generateBa();
25327 final double biasX = ba.getElementAtIndex(0);
25328 final double biasY = ba.getElementAtIndex(1);
25329 final double biasZ = ba.getElementAtIndex(2);
25330
25331 final Matrix ma = generateMaCommonAxis();
25332 final double sx = ma.getElementAt(0, 0);
25333 final double sy = ma.getElementAt(1, 1);
25334 final double sz = ma.getElementAt(2, 2);
25335 final double mxy = ma.getElementAt(0, 1);
25336 final double mxz = ma.getElementAt(0, 2);
25337 final double myx = ma.getElementAt(1, 0);
25338 final double myz = ma.getElementAt(1, 2);
25339 final double mzx = ma.getElementAt(2, 0);
25340 final double mzy = ma.getElementAt(2, 1);
25341
25342 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25343 final double latitude = Math.toRadians(
25344 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25345 final double longitude = Math.toRadians(
25346 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25347 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25348 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25349 final NEDVelocity nedVelocity = new NEDVelocity();
25350 final ECEFPosition ecefPosition = new ECEFPosition();
25351 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25352 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25353 ecefPosition, ecefVelocity);
25354
25355 final KnownPositionAccelerometerCalibrator calibrator =
25356 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25357 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25358 myx, myz, mzx, mzy, this);
25359
25360
25361 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25362 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25363 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25364 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25365 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25366 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25367 final Acceleration bx2 = new Acceleration(0.0,
25368 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25369 calibrator.getInitialBiasXAsAcceleration(bx2);
25370 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25371 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25372 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25373 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25374 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25375 final Acceleration by2 = new Acceleration(0.0,
25376 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25377 calibrator.getInitialBiasYAsAcceleration(by2);
25378 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25379 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25380 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25381 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25382 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25383 final Acceleration bz2 = new Acceleration(0.0,
25384 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25385 calibrator.getInitialBiasZAsAcceleration(bz2);
25386 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25387 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25388 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25389 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25390 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25391 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25392 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25393 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25394 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25395 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25396 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25397 final double[] bias1 = calibrator.getInitialBias();
25398 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25399 final double[] bias2 = new double[3];
25400 calibrator.getInitialBias(bias2);
25401 assertArrayEquals(bias1, bias2, 0.0);
25402 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25403 assertEquals(b1, ba);
25404 final Matrix b2 = new Matrix(3, 1);
25405 calibrator.getInitialBiasAsMatrix(b2);
25406 assertEquals(b1, b2);
25407 final Matrix ma1 = new Matrix(3, 3);
25408 ma1.setSubmatrix(0, 0,
25409 2, 2,
25410 new double[]{sx, myx, mzx,
25411 mxy, sy, mzy,
25412 mxz, myz, sz});
25413 assertEquals(calibrator.getInitialMa(), ma1);
25414 final Matrix ma2 = new Matrix(3, 3);
25415 calibrator.getInitialMa(ma2);
25416 assertEquals(ma1, ma2);
25417 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25418 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25419 final NEDPosition nedPosition1 = new NEDPosition();
25420 assertTrue(calibrator.getNedPosition(nedPosition1));
25421 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25422 assertSame(calibrator.getMeasurements(), measurements);
25423 assertFalse(calibrator.isCommonAxisUsed());
25424 assertSame(calibrator.getListener(), this);
25425 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25426 assertFalse(calibrator.isReady());
25427 assertFalse(calibrator.isRunning());
25428 assertNull(calibrator.getEstimatedBiases());
25429 assertFalse(calibrator.getEstimatedBiases(null));
25430 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25431 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25432 assertNull(calibrator.getEstimatedBiasFx());
25433 assertNull(calibrator.getEstimatedBiasFy());
25434 assertNull(calibrator.getEstimatedBiasFz());
25435 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25436 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25437 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25438 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25439 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25440 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25441 assertNull(calibrator.getEstimatedMa());
25442 assertNull(calibrator.getEstimatedSx());
25443 assertNull(calibrator.getEstimatedSy());
25444 assertNull(calibrator.getEstimatedSz());
25445 assertNull(calibrator.getEstimatedMxy());
25446 assertNull(calibrator.getEstimatedMxz());
25447 assertNull(calibrator.getEstimatedMyx());
25448 assertNull(calibrator.getEstimatedMyz());
25449 assertNull(calibrator.getEstimatedMzx());
25450 assertNull(calibrator.getEstimatedMzy());
25451 assertNull(calibrator.getEstimatedCovariance());
25452 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25453 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25454 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25455 assertNotNull(calibrator.getGroundTruthGravityNorm());
25456 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25457 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25458 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25459 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25460 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25461 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25462 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25463 }
25464
25465 @Test
25466 public void testConstructor199() throws WrongSizeException {
25467 final Matrix ba = generateBa();
25468 final double biasX = ba.getElementAtIndex(0);
25469 final double biasY = ba.getElementAtIndex(1);
25470 final double biasZ = ba.getElementAtIndex(2);
25471
25472 final Matrix ma = generateMaCommonAxis();
25473 final double sx = ma.getElementAt(0, 0);
25474 final double sy = ma.getElementAt(1, 1);
25475 final double sz = ma.getElementAt(2, 2);
25476 final double mxy = ma.getElementAt(0, 1);
25477 final double mxz = ma.getElementAt(0, 2);
25478 final double myx = ma.getElementAt(1, 0);
25479 final double myz = ma.getElementAt(1, 2);
25480 final double mzx = ma.getElementAt(2, 0);
25481 final double mzy = ma.getElementAt(2, 1);
25482
25483 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25484 final double latitude = Math.toRadians(
25485 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25486 final double longitude = Math.toRadians(
25487 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25488 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25489 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25490 final NEDVelocity nedVelocity = new NEDVelocity();
25491 final ECEFPosition ecefPosition = new ECEFPosition();
25492 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25493 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25494 ecefPosition, ecefVelocity);
25495
25496 final KnownPositionAccelerometerCalibrator calibrator =
25497 new KnownPositionAccelerometerCalibrator(nedPosition,
25498 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25499 myx, myz, mzx, mzy);
25500
25501
25502 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25503 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25504 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25505 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25506 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25507 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25508 final Acceleration bx2 = new Acceleration(0.0,
25509 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25510 calibrator.getInitialBiasXAsAcceleration(bx2);
25511 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25512 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25513 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25514 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25515 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25516 final Acceleration by2 = new Acceleration(0.0,
25517 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25518 calibrator.getInitialBiasYAsAcceleration(by2);
25519 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25520 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25521 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25522 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25523 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25524 final Acceleration bz2 = new Acceleration(0.0,
25525 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25526 calibrator.getInitialBiasZAsAcceleration(bz2);
25527 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25528 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25529 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25530 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25531 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25532 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25533 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25534 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25535 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25536 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25537 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25538 final double[] bias1 = calibrator.getInitialBias();
25539 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25540 final double[] bias2 = new double[3];
25541 calibrator.getInitialBias(bias2);
25542 assertArrayEquals(bias1, bias2, 0.0);
25543 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25544 assertEquals(b1, ba);
25545 final Matrix b2 = new Matrix(3, 1);
25546 calibrator.getInitialBiasAsMatrix(b2);
25547 assertEquals(b1, b2);
25548 final Matrix ma1 = new Matrix(3, 3);
25549 ma1.setSubmatrix(0, 0,
25550 2, 2,
25551 new double[]{sx, myx, mzx,
25552 mxy, sy, mzy,
25553 mxz, myz, sz});
25554 assertEquals(calibrator.getInitialMa(), ma1);
25555 final Matrix ma2 = new Matrix(3, 3);
25556 calibrator.getInitialMa(ma2);
25557 assertEquals(ma1, ma2);
25558 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25559 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25560 final NEDPosition nedPosition1 = new NEDPosition();
25561 assertTrue(calibrator.getNedPosition(nedPosition1));
25562 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25563 assertNull(calibrator.getMeasurements());
25564 assertTrue(calibrator.isCommonAxisUsed());
25565 assertNull(calibrator.getListener());
25566 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25567 assertFalse(calibrator.isReady());
25568 assertFalse(calibrator.isRunning());
25569 assertNull(calibrator.getEstimatedBiases());
25570 assertFalse(calibrator.getEstimatedBiases(null));
25571 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25572 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25573 assertNull(calibrator.getEstimatedBiasFx());
25574 assertNull(calibrator.getEstimatedBiasFy());
25575 assertNull(calibrator.getEstimatedBiasFz());
25576 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25577 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25578 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25579 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25580 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25581 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25582 assertNull(calibrator.getEstimatedMa());
25583 assertNull(calibrator.getEstimatedSx());
25584 assertNull(calibrator.getEstimatedSy());
25585 assertNull(calibrator.getEstimatedSz());
25586 assertNull(calibrator.getEstimatedMxy());
25587 assertNull(calibrator.getEstimatedMxz());
25588 assertNull(calibrator.getEstimatedMyx());
25589 assertNull(calibrator.getEstimatedMyz());
25590 assertNull(calibrator.getEstimatedMzx());
25591 assertNull(calibrator.getEstimatedMzy());
25592 assertNull(calibrator.getEstimatedCovariance());
25593 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25594 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25595 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25596 assertNotNull(calibrator.getGroundTruthGravityNorm());
25597 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25598 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25599 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25600 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25601 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25602 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25603 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25604 }
25605
25606 @Test
25607 public void testConstructor200() throws WrongSizeException {
25608 final Matrix ba = generateBa();
25609 final double biasX = ba.getElementAtIndex(0);
25610 final double biasY = ba.getElementAtIndex(1);
25611 final double biasZ = ba.getElementAtIndex(2);
25612
25613 final Matrix ma = generateMaCommonAxis();
25614 final double sx = ma.getElementAt(0, 0);
25615 final double sy = ma.getElementAt(1, 1);
25616 final double sz = ma.getElementAt(2, 2);
25617 final double mxy = ma.getElementAt(0, 1);
25618 final double mxz = ma.getElementAt(0, 2);
25619 final double myx = ma.getElementAt(1, 0);
25620 final double myz = ma.getElementAt(1, 2);
25621 final double mzx = ma.getElementAt(2, 0);
25622 final double mzy = ma.getElementAt(2, 1);
25623
25624 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25625 final double latitude = Math.toRadians(
25626 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25627 final double longitude = Math.toRadians(
25628 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25629 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25630 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25631 final NEDVelocity nedVelocity = new NEDVelocity();
25632 final ECEFPosition ecefPosition = new ECEFPosition();
25633 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25634 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25635 ecefPosition, ecefVelocity);
25636
25637 final KnownPositionAccelerometerCalibrator calibrator =
25638 new KnownPositionAccelerometerCalibrator(nedPosition,
25639 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25640 myx, myz, mzx, mzy, this);
25641
25642
25643 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25644 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25645 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25646 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25647 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25648 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25649 final Acceleration bx2 = new Acceleration(0.0,
25650 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25651 calibrator.getInitialBiasXAsAcceleration(bx2);
25652 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25653 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25654 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25655 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25656 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25657 final Acceleration by2 = new Acceleration(0.0,
25658 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25659 calibrator.getInitialBiasYAsAcceleration(by2);
25660 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25661 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25662 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25663 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25664 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25665 final Acceleration bz2 = new Acceleration(0.0,
25666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25667 calibrator.getInitialBiasZAsAcceleration(bz2);
25668 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25669 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25670 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25671 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25672 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25673 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25674 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25675 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25676 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25677 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25678 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25679 final double[] bias1 = calibrator.getInitialBias();
25680 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25681 final double[] bias2 = new double[3];
25682 calibrator.getInitialBias(bias2);
25683 assertArrayEquals(bias1, bias2, 0.0);
25684 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25685 assertEquals(b1, ba);
25686 final Matrix b2 = new Matrix(3, 1);
25687 calibrator.getInitialBiasAsMatrix(b2);
25688 assertEquals(b1, b2);
25689 final Matrix ma1 = new Matrix(3, 3);
25690 ma1.setSubmatrix(0, 0,
25691 2, 2,
25692 new double[]{sx, myx, mzx,
25693 mxy, sy, mzy,
25694 mxz, myz, sz});
25695 assertEquals(calibrator.getInitialMa(), ma1);
25696 final Matrix ma2 = new Matrix(3, 3);
25697 calibrator.getInitialMa(ma2);
25698 assertEquals(ma1, ma2);
25699 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25700 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25701 final NEDPosition nedPosition1 = new NEDPosition();
25702 assertTrue(calibrator.getNedPosition(nedPosition1));
25703 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25704 assertNull(calibrator.getMeasurements());
25705 assertTrue(calibrator.isCommonAxisUsed());
25706 assertSame(calibrator.getListener(), this);
25707 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25708 assertFalse(calibrator.isReady());
25709 assertFalse(calibrator.isRunning());
25710 assertNull(calibrator.getEstimatedBiases());
25711 assertFalse(calibrator.getEstimatedBiases(null));
25712 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25713 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25714 assertNull(calibrator.getEstimatedBiasFx());
25715 assertNull(calibrator.getEstimatedBiasFy());
25716 assertNull(calibrator.getEstimatedBiasFz());
25717 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25718 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25719 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25720 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25721 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25722 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25723 assertNull(calibrator.getEstimatedMa());
25724 assertNull(calibrator.getEstimatedSx());
25725 assertNull(calibrator.getEstimatedSy());
25726 assertNull(calibrator.getEstimatedSz());
25727 assertNull(calibrator.getEstimatedMxy());
25728 assertNull(calibrator.getEstimatedMxz());
25729 assertNull(calibrator.getEstimatedMyx());
25730 assertNull(calibrator.getEstimatedMyz());
25731 assertNull(calibrator.getEstimatedMzx());
25732 assertNull(calibrator.getEstimatedMzy());
25733 assertNull(calibrator.getEstimatedCovariance());
25734 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25735 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25736 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25737 assertNotNull(calibrator.getGroundTruthGravityNorm());
25738 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25739 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25740 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25741 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25742 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25743 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25744 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25745 }
25746
25747 @Test
25748 public void testConstructor201() throws WrongSizeException {
25749 final Collection<StandardDeviationBodyKinematics> measurements =
25750 Collections.emptyList();
25751
25752 final Matrix ba = generateBa();
25753 final double biasX = ba.getElementAtIndex(0);
25754 final double biasY = ba.getElementAtIndex(1);
25755 final double biasZ = ba.getElementAtIndex(2);
25756
25757 final Matrix ma = generateMaCommonAxis();
25758 final double sx = ma.getElementAt(0, 0);
25759 final double sy = ma.getElementAt(1, 1);
25760 final double sz = ma.getElementAt(2, 2);
25761 final double mxy = ma.getElementAt(0, 1);
25762 final double mxz = ma.getElementAt(0, 2);
25763 final double myx = ma.getElementAt(1, 0);
25764 final double myz = ma.getElementAt(1, 2);
25765 final double mzx = ma.getElementAt(2, 0);
25766 final double mzy = ma.getElementAt(2, 1);
25767
25768 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25769 final double latitude = Math.toRadians(
25770 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25771 final double longitude = Math.toRadians(
25772 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25773 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25774 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25775 final NEDVelocity nedVelocity = new NEDVelocity();
25776 final ECEFPosition ecefPosition = new ECEFPosition();
25777 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25778 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25779 ecefPosition, ecefVelocity);
25780
25781 final KnownPositionAccelerometerCalibrator calibrator =
25782 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25783 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25784 myx, myz, mzx, mzy);
25785
25786
25787 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25788 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25789 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25790 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25791 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25792 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25793 final Acceleration bx2 = new Acceleration(0.0,
25794 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25795 calibrator.getInitialBiasXAsAcceleration(bx2);
25796 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25797 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25798 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25799 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25800 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25801 final Acceleration by2 = new Acceleration(0.0,
25802 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25803 calibrator.getInitialBiasYAsAcceleration(by2);
25804 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25805 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25806 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25807 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25808 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25809 final Acceleration bz2 = new Acceleration(0.0,
25810 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25811 calibrator.getInitialBiasZAsAcceleration(bz2);
25812 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25813 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25814 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25815 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25816 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25817 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25818 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25819 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25820 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25821 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25822 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25823 final double[] bias1 = calibrator.getInitialBias();
25824 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25825 final double[] bias2 = new double[3];
25826 calibrator.getInitialBias(bias2);
25827 assertArrayEquals(bias1, bias2, 0.0);
25828 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25829 assertEquals(b1, ba);
25830 final Matrix b2 = new Matrix(3, 1);
25831 calibrator.getInitialBiasAsMatrix(b2);
25832 assertEquals(b1, b2);
25833 final Matrix ma1 = new Matrix(3, 3);
25834 ma1.setSubmatrix(0, 0,
25835 2, 2,
25836 new double[]{sx, myx, mzx,
25837 mxy, sy, mzy,
25838 mxz, myz, sz});
25839 assertEquals(calibrator.getInitialMa(), ma1);
25840 final Matrix ma2 = new Matrix(3, 3);
25841 calibrator.getInitialMa(ma2);
25842 assertEquals(ma1, ma2);
25843 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25844 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25845 final NEDPosition nedPosition1 = new NEDPosition();
25846 assertTrue(calibrator.getNedPosition(nedPosition1));
25847 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25848 assertSame(calibrator.getMeasurements(), measurements);
25849 assertTrue(calibrator.isCommonAxisUsed());
25850 assertNull(calibrator.getListener());
25851 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25852 assertFalse(calibrator.isReady());
25853 assertFalse(calibrator.isRunning());
25854 assertNull(calibrator.getEstimatedBiases());
25855 assertFalse(calibrator.getEstimatedBiases(null));
25856 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25857 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25858 assertNull(calibrator.getEstimatedBiasFx());
25859 assertNull(calibrator.getEstimatedBiasFy());
25860 assertNull(calibrator.getEstimatedBiasFz());
25861 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25862 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25863 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25864 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25865 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25866 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25867 assertNull(calibrator.getEstimatedMa());
25868 assertNull(calibrator.getEstimatedSx());
25869 assertNull(calibrator.getEstimatedSy());
25870 assertNull(calibrator.getEstimatedSz());
25871 assertNull(calibrator.getEstimatedMxy());
25872 assertNull(calibrator.getEstimatedMxz());
25873 assertNull(calibrator.getEstimatedMyx());
25874 assertNull(calibrator.getEstimatedMyz());
25875 assertNull(calibrator.getEstimatedMzx());
25876 assertNull(calibrator.getEstimatedMzy());
25877 assertNull(calibrator.getEstimatedCovariance());
25878 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25879 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25880 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25881 assertNotNull(calibrator.getGroundTruthGravityNorm());
25882 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25883 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25884 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25885 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25886 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25887 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25888 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25889 }
25890
25891 @Test
25892 public void testConstructor202() throws WrongSizeException {
25893 final Collection<StandardDeviationBodyKinematics> measurements =
25894 Collections.emptyList();
25895
25896 final Matrix ba = generateBa();
25897 final double biasX = ba.getElementAtIndex(0);
25898 final double biasY = ba.getElementAtIndex(1);
25899 final double biasZ = ba.getElementAtIndex(2);
25900
25901 final Matrix ma = generateMaCommonAxis();
25902 final double sx = ma.getElementAt(0, 0);
25903 final double sy = ma.getElementAt(1, 1);
25904 final double sz = ma.getElementAt(2, 2);
25905 final double mxy = ma.getElementAt(0, 1);
25906 final double mxz = ma.getElementAt(0, 2);
25907 final double myx = ma.getElementAt(1, 0);
25908 final double myz = ma.getElementAt(1, 2);
25909 final double mzx = ma.getElementAt(2, 0);
25910 final double mzy = ma.getElementAt(2, 1);
25911
25912 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25913 final double latitude = Math.toRadians(
25914 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25915 final double longitude = Math.toRadians(
25916 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25917 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25918 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25919 final NEDVelocity nedVelocity = new NEDVelocity();
25920 final ECEFPosition ecefPosition = new ECEFPosition();
25921 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25922 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25923 ecefPosition, ecefVelocity);
25924
25925 final KnownPositionAccelerometerCalibrator calibrator =
25926 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25927 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25928 myx, myz, mzx, mzy, this);
25929
25930
25931 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25932 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25933 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25934 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25935 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25936 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25937 final Acceleration bx2 = new Acceleration(0.0,
25938 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25939 calibrator.getInitialBiasXAsAcceleration(bx2);
25940 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25941 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25942 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25943 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25944 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25945 final Acceleration by2 = new Acceleration(0.0,
25946 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25947 calibrator.getInitialBiasYAsAcceleration(by2);
25948 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25949 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25950 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25951 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25952 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25953 final Acceleration bz2 = new Acceleration(0.0,
25954 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25955 calibrator.getInitialBiasZAsAcceleration(bz2);
25956 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25957 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25958 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25959 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25960 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25961 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25962 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25963 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25964 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25965 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25966 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25967 final double[] bias1 = calibrator.getInitialBias();
25968 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25969 final double[] bias2 = new double[3];
25970 calibrator.getInitialBias(bias2);
25971 assertArrayEquals(bias1, bias2, 0.0);
25972 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25973 assertEquals(b1, ba);
25974 final Matrix b2 = new Matrix(3, 1);
25975 calibrator.getInitialBiasAsMatrix(b2);
25976 assertEquals(b1, b2);
25977 final Matrix ma1 = new Matrix(3, 3);
25978 ma1.setSubmatrix(0, 0,
25979 2, 2,
25980 new double[]{sx, myx, mzx,
25981 mxy, sy, mzy,
25982 mxz, myz, sz});
25983 assertEquals(calibrator.getInitialMa(), ma1);
25984 final Matrix ma2 = new Matrix(3, 3);
25985 calibrator.getInitialMa(ma2);
25986 assertEquals(ma1, ma2);
25987 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25988 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25989 final NEDPosition nedPosition1 = new NEDPosition();
25990 assertTrue(calibrator.getNedPosition(nedPosition1));
25991 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25992 assertSame(calibrator.getMeasurements(), measurements);
25993 assertTrue(calibrator.isCommonAxisUsed());
25994 assertSame(calibrator.getListener(), this);
25995 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25996 assertFalse(calibrator.isReady());
25997 assertFalse(calibrator.isRunning());
25998 assertNull(calibrator.getEstimatedBiases());
25999 assertFalse(calibrator.getEstimatedBiases(null));
26000 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26001 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26002 assertNull(calibrator.getEstimatedBiasFx());
26003 assertNull(calibrator.getEstimatedBiasFy());
26004 assertNull(calibrator.getEstimatedBiasFz());
26005 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26006 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26007 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26008 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26009 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26010 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26011 assertNull(calibrator.getEstimatedMa());
26012 assertNull(calibrator.getEstimatedSx());
26013 assertNull(calibrator.getEstimatedSy());
26014 assertNull(calibrator.getEstimatedSz());
26015 assertNull(calibrator.getEstimatedMxy());
26016 assertNull(calibrator.getEstimatedMxz());
26017 assertNull(calibrator.getEstimatedMyx());
26018 assertNull(calibrator.getEstimatedMyz());
26019 assertNull(calibrator.getEstimatedMzx());
26020 assertNull(calibrator.getEstimatedMzy());
26021 assertNull(calibrator.getEstimatedCovariance());
26022 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26023 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26024 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26025 assertNotNull(calibrator.getGroundTruthGravityNorm());
26026 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26027 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26028 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26029 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26030 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26031 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26032 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26033 }
26034
26035 @Test
26036 public void testConstructor203() throws WrongSizeException {
26037 final Matrix ba = generateBa();
26038 final double biasX = ba.getElementAtIndex(0);
26039 final double biasY = ba.getElementAtIndex(1);
26040 final double biasZ = ba.getElementAtIndex(2);
26041
26042 final Acceleration bx = new Acceleration(biasX,
26043 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26044 final Acceleration by = new Acceleration(biasY,
26045 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26046 final Acceleration bz = new Acceleration(biasZ,
26047 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26048
26049 final Matrix ma = generateMaCommonAxis();
26050 final double sx = ma.getElementAt(0, 0);
26051 final double sy = ma.getElementAt(1, 1);
26052 final double sz = ma.getElementAt(2, 2);
26053 final double mxy = ma.getElementAt(0, 1);
26054 final double mxz = ma.getElementAt(0, 2);
26055 final double myx = ma.getElementAt(1, 0);
26056 final double myz = ma.getElementAt(1, 2);
26057 final double mzx = ma.getElementAt(2, 0);
26058 final double mzy = ma.getElementAt(2, 1);
26059
26060 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26061 final double latitude = Math.toRadians(
26062 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26063 final double longitude = Math.toRadians(
26064 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26065 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26066 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26067 final NEDVelocity nedVelocity = new NEDVelocity();
26068 final ECEFPosition ecefPosition = new ECEFPosition();
26069 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26070 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26071 ecefPosition, ecefVelocity);
26072
26073 final KnownPositionAccelerometerCalibrator calibrator =
26074 new KnownPositionAccelerometerCalibrator(nedPosition,
26075 bx, by, bz, sx, sy, sz, mxy, mxz,
26076 myx, myz, mzx, mzy);
26077
26078
26079 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26080 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26081 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26082 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26083 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26084 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26085 final Acceleration bx2 = new Acceleration(0.0,
26086 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26087 calibrator.getInitialBiasXAsAcceleration(bx2);
26088 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26089 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26090 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26091 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26092 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26093 final Acceleration by2 = new Acceleration(0.0,
26094 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26095 calibrator.getInitialBiasYAsAcceleration(by2);
26096 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26097 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26098 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26099 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26100 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26101 final Acceleration bz2 = new Acceleration(0.0,
26102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26103 calibrator.getInitialBiasZAsAcceleration(bz2);
26104 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26105 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26106 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26107 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26108 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26109 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26110 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26111 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26112 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26113 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26114 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26115 final double[] bias1 = calibrator.getInitialBias();
26116 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26117 final double[] bias2 = new double[3];
26118 calibrator.getInitialBias(bias2);
26119 assertArrayEquals(bias1, bias2, 0.0);
26120 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26121 assertEquals(b1, ba);
26122 final Matrix b2 = new Matrix(3, 1);
26123 calibrator.getInitialBiasAsMatrix(b2);
26124 assertEquals(b1, b2);
26125 final Matrix ma1 = new Matrix(3, 3);
26126 ma1.setSubmatrix(0, 0,
26127 2, 2,
26128 new double[]{sx, myx, mzx,
26129 mxy, sy, mzy,
26130 mxz, myz, sz});
26131 assertEquals(calibrator.getInitialMa(), ma1);
26132 final Matrix ma2 = new Matrix(3, 3);
26133 calibrator.getInitialMa(ma2);
26134 assertEquals(ma1, ma2);
26135 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26136 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26137 final NEDPosition nedPosition1 = new NEDPosition();
26138 assertTrue(calibrator.getNedPosition(nedPosition1));
26139 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26140 assertNull(calibrator.getMeasurements());
26141 assertFalse(calibrator.isCommonAxisUsed());
26142 assertNull(calibrator.getListener());
26143 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26144 assertFalse(calibrator.isReady());
26145 assertFalse(calibrator.isRunning());
26146 assertNull(calibrator.getEstimatedBiases());
26147 assertFalse(calibrator.getEstimatedBiases(null));
26148 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26149 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26150 assertNull(calibrator.getEstimatedBiasFx());
26151 assertNull(calibrator.getEstimatedBiasFy());
26152 assertNull(calibrator.getEstimatedBiasFz());
26153 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26154 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26155 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26156 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26157 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26158 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26159 assertNull(calibrator.getEstimatedMa());
26160 assertNull(calibrator.getEstimatedSx());
26161 assertNull(calibrator.getEstimatedSy());
26162 assertNull(calibrator.getEstimatedSz());
26163 assertNull(calibrator.getEstimatedMxy());
26164 assertNull(calibrator.getEstimatedMxz());
26165 assertNull(calibrator.getEstimatedMyx());
26166 assertNull(calibrator.getEstimatedMyz());
26167 assertNull(calibrator.getEstimatedMzx());
26168 assertNull(calibrator.getEstimatedMzy());
26169 assertNull(calibrator.getEstimatedCovariance());
26170 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26171 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26172 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26173 assertNotNull(calibrator.getGroundTruthGravityNorm());
26174 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26175 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26176 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26177 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26178 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26179 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26180 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26181 }
26182
26183 @Test
26184 public void testConstructor204() throws WrongSizeException {
26185 final Matrix ba = generateBa();
26186 final double biasX = ba.getElementAtIndex(0);
26187 final double biasY = ba.getElementAtIndex(1);
26188 final double biasZ = ba.getElementAtIndex(2);
26189
26190 final Acceleration bx = new Acceleration(biasX,
26191 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26192 final Acceleration by = new Acceleration(biasY,
26193 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26194 final Acceleration bz = new Acceleration(biasZ,
26195 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26196
26197 final Matrix ma = generateMaCommonAxis();
26198 final double sx = ma.getElementAt(0, 0);
26199 final double sy = ma.getElementAt(1, 1);
26200 final double sz = ma.getElementAt(2, 2);
26201 final double mxy = ma.getElementAt(0, 1);
26202 final double mxz = ma.getElementAt(0, 2);
26203 final double myx = ma.getElementAt(1, 0);
26204 final double myz = ma.getElementAt(1, 2);
26205 final double mzx = ma.getElementAt(2, 0);
26206 final double mzy = ma.getElementAt(2, 1);
26207
26208 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26209 final double latitude = Math.toRadians(
26210 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26211 final double longitude = Math.toRadians(
26212 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26213 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26214 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26215 final NEDVelocity nedVelocity = new NEDVelocity();
26216 final ECEFPosition ecefPosition = new ECEFPosition();
26217 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26218 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26219 ecefPosition, ecefVelocity);
26220
26221 final KnownPositionAccelerometerCalibrator calibrator =
26222 new KnownPositionAccelerometerCalibrator(nedPosition,
26223 bx, by, bz, sx, sy, sz, mxy, mxz,
26224 myx, myz, mzx, mzy, this);
26225
26226
26227 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26228 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26229 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26230 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26231 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26232 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26233 final Acceleration bx2 = new Acceleration(0.0,
26234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26235 calibrator.getInitialBiasXAsAcceleration(bx2);
26236 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26237 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26238 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26239 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26240 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26241 final Acceleration by2 = new Acceleration(0.0,
26242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26243 calibrator.getInitialBiasYAsAcceleration(by2);
26244 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26245 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26246 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26247 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26248 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26249 final Acceleration bz2 = new Acceleration(0.0,
26250 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26251 calibrator.getInitialBiasZAsAcceleration(bz2);
26252 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26253 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26254 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26255 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26256 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26257 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26258 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26259 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26260 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26261 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26262 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26263 final double[] bias1 = calibrator.getInitialBias();
26264 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26265 final double[] bias2 = new double[3];
26266 calibrator.getInitialBias(bias2);
26267 assertArrayEquals(bias1, bias2, 0.0);
26268 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26269 assertEquals(b1, ba);
26270 final Matrix b2 = new Matrix(3, 1);
26271 calibrator.getInitialBiasAsMatrix(b2);
26272 assertEquals(b1, b2);
26273 final Matrix ma1 = new Matrix(3, 3);
26274 ma1.setSubmatrix(0, 0,
26275 2, 2,
26276 new double[]{sx, myx, mzx,
26277 mxy, sy, mzy,
26278 mxz, myz, sz});
26279 assertEquals(calibrator.getInitialMa(), ma1);
26280 final Matrix ma2 = new Matrix(3, 3);
26281 calibrator.getInitialMa(ma2);
26282 assertEquals(ma1, ma2);
26283 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26284 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26285 final NEDPosition nedPosition1 = new NEDPosition();
26286 assertTrue(calibrator.getNedPosition(nedPosition1));
26287 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26288 assertNull(calibrator.getMeasurements());
26289 assertFalse(calibrator.isCommonAxisUsed());
26290 assertSame(calibrator.getListener(), this);
26291 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26292 assertFalse(calibrator.isReady());
26293 assertFalse(calibrator.isRunning());
26294 assertNull(calibrator.getEstimatedBiases());
26295 assertFalse(calibrator.getEstimatedBiases(null));
26296 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26297 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26298 assertNull(calibrator.getEstimatedBiasFx());
26299 assertNull(calibrator.getEstimatedBiasFy());
26300 assertNull(calibrator.getEstimatedBiasFz());
26301 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26302 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26303 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26304 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26305 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26306 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26307 assertNull(calibrator.getEstimatedMa());
26308 assertNull(calibrator.getEstimatedSx());
26309 assertNull(calibrator.getEstimatedSy());
26310 assertNull(calibrator.getEstimatedSz());
26311 assertNull(calibrator.getEstimatedMxy());
26312 assertNull(calibrator.getEstimatedMxz());
26313 assertNull(calibrator.getEstimatedMyx());
26314 assertNull(calibrator.getEstimatedMyz());
26315 assertNull(calibrator.getEstimatedMzx());
26316 assertNull(calibrator.getEstimatedMzy());
26317 assertNull(calibrator.getEstimatedCovariance());
26318 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26319 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26320 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26321 assertNotNull(calibrator.getGroundTruthGravityNorm());
26322 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26323 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26324 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26325 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26326 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26327 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26328 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26329 }
26330
26331 @Test
26332 public void testConstructor205() throws WrongSizeException {
26333 final Collection<StandardDeviationBodyKinematics> measurements =
26334 Collections.emptyList();
26335
26336 final Matrix ba = generateBa();
26337 final double biasX = ba.getElementAtIndex(0);
26338 final double biasY = ba.getElementAtIndex(1);
26339 final double biasZ = ba.getElementAtIndex(2);
26340
26341 final Acceleration bx = new Acceleration(biasX,
26342 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26343 final Acceleration by = new Acceleration(biasY,
26344 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26345 final Acceleration bz = new Acceleration(biasZ,
26346 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26347
26348 final Matrix ma = generateMaCommonAxis();
26349 final double sx = ma.getElementAt(0, 0);
26350 final double sy = ma.getElementAt(1, 1);
26351 final double sz = ma.getElementAt(2, 2);
26352 final double mxy = ma.getElementAt(0, 1);
26353 final double mxz = ma.getElementAt(0, 2);
26354 final double myx = ma.getElementAt(1, 0);
26355 final double myz = ma.getElementAt(1, 2);
26356 final double mzx = ma.getElementAt(2, 0);
26357 final double mzy = ma.getElementAt(2, 1);
26358
26359 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26360 final double latitude = Math.toRadians(
26361 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26362 final double longitude = Math.toRadians(
26363 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26364 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26365 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26366 final NEDVelocity nedVelocity = new NEDVelocity();
26367 final ECEFPosition ecefPosition = new ECEFPosition();
26368 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26369 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26370 ecefPosition, ecefVelocity);
26371
26372 final KnownPositionAccelerometerCalibrator calibrator =
26373 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26374 bx, by, bz, sx, sy, sz, mxy, mxz,
26375 myx, myz, mzx, mzy);
26376
26377
26378 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26379 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26380 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26381 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26382 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26383 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26384 final Acceleration bx2 = new Acceleration(0.0,
26385 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26386 calibrator.getInitialBiasXAsAcceleration(bx2);
26387 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26388 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26389 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26390 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26391 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26392 final Acceleration by2 = new Acceleration(0.0,
26393 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26394 calibrator.getInitialBiasYAsAcceleration(by2);
26395 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26396 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26397 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26398 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26399 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26400 final Acceleration bz2 = new Acceleration(0.0,
26401 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26402 calibrator.getInitialBiasZAsAcceleration(bz2);
26403 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26404 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26405 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26406 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26407 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26408 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26409 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26410 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26411 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26412 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26413 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26414 final double[] bias1 = calibrator.getInitialBias();
26415 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26416 final double[] bias2 = new double[3];
26417 calibrator.getInitialBias(bias2);
26418 assertArrayEquals(bias1, bias2, 0.0);
26419 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26420 assertEquals(b1, ba);
26421 final Matrix b2 = new Matrix(3, 1);
26422 calibrator.getInitialBiasAsMatrix(b2);
26423 assertEquals(b1, b2);
26424 final Matrix ma1 = new Matrix(3, 3);
26425 ma1.setSubmatrix(0, 0,
26426 2, 2,
26427 new double[]{sx, myx, mzx,
26428 mxy, sy, mzy,
26429 mxz, myz, sz});
26430 assertEquals(calibrator.getInitialMa(), ma1);
26431 final Matrix ma2 = new Matrix(3, 3);
26432 calibrator.getInitialMa(ma2);
26433 assertEquals(ma1, ma2);
26434 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26435 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26436 final NEDPosition nedPosition1 = new NEDPosition();
26437 assertTrue(calibrator.getNedPosition(nedPosition1));
26438 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26439 assertSame(calibrator.getMeasurements(), measurements);
26440 assertFalse(calibrator.isCommonAxisUsed());
26441 assertNull(calibrator.getListener());
26442 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26443 assertFalse(calibrator.isReady());
26444 assertFalse(calibrator.isRunning());
26445 assertNull(calibrator.getEstimatedBiases());
26446 assertFalse(calibrator.getEstimatedBiases(null));
26447 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26448 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26449 assertNull(calibrator.getEstimatedBiasFx());
26450 assertNull(calibrator.getEstimatedBiasFy());
26451 assertNull(calibrator.getEstimatedBiasFz());
26452 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26453 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26454 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26455 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26456 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26457 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26458 assertNull(calibrator.getEstimatedMa());
26459 assertNull(calibrator.getEstimatedSx());
26460 assertNull(calibrator.getEstimatedSy());
26461 assertNull(calibrator.getEstimatedSz());
26462 assertNull(calibrator.getEstimatedMxy());
26463 assertNull(calibrator.getEstimatedMxz());
26464 assertNull(calibrator.getEstimatedMyx());
26465 assertNull(calibrator.getEstimatedMyz());
26466 assertNull(calibrator.getEstimatedMzx());
26467 assertNull(calibrator.getEstimatedMzy());
26468 assertNull(calibrator.getEstimatedCovariance());
26469 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26470 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26471 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26472 assertNotNull(calibrator.getGroundTruthGravityNorm());
26473 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26474 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26475 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26476 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26477 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26478 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26479 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26480 }
26481
26482 @Test
26483 public void testConstructor206() throws WrongSizeException {
26484 final Collection<StandardDeviationBodyKinematics> measurements =
26485 Collections.emptyList();
26486
26487 final Matrix ba = generateBa();
26488 final double biasX = ba.getElementAtIndex(0);
26489 final double biasY = ba.getElementAtIndex(1);
26490 final double biasZ = ba.getElementAtIndex(2);
26491
26492 final Acceleration bx = new Acceleration(biasX,
26493 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26494 final Acceleration by = new Acceleration(biasY,
26495 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26496 final Acceleration bz = new Acceleration(biasZ,
26497 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26498
26499 final Matrix ma = generateMaCommonAxis();
26500 final double sx = ma.getElementAt(0, 0);
26501 final double sy = ma.getElementAt(1, 1);
26502 final double sz = ma.getElementAt(2, 2);
26503 final double mxy = ma.getElementAt(0, 1);
26504 final double mxz = ma.getElementAt(0, 2);
26505 final double myx = ma.getElementAt(1, 0);
26506 final double myz = ma.getElementAt(1, 2);
26507 final double mzx = ma.getElementAt(2, 0);
26508 final double mzy = ma.getElementAt(2, 1);
26509
26510 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26511 final double latitude = Math.toRadians(
26512 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26513 final double longitude = Math.toRadians(
26514 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26515 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26516 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26517 final NEDVelocity nedVelocity = new NEDVelocity();
26518 final ECEFPosition ecefPosition = new ECEFPosition();
26519 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26520 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26521 ecefPosition, ecefVelocity);
26522
26523 final KnownPositionAccelerometerCalibrator calibrator =
26524 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26525 bx, by, bz, sx, sy, sz, mxy, mxz,
26526 myx, myz, mzx, mzy, this);
26527
26528
26529 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26530 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26531 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26532 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26533 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26534 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26535 final Acceleration bx2 = new Acceleration(0.0,
26536 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26537 calibrator.getInitialBiasXAsAcceleration(bx2);
26538 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26539 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26540 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26541 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26542 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26543 final Acceleration by2 = new Acceleration(0.0,
26544 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26545 calibrator.getInitialBiasYAsAcceleration(by2);
26546 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26547 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26548 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26549 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26550 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26551 final Acceleration bz2 = new Acceleration(0.0,
26552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26553 calibrator.getInitialBiasZAsAcceleration(bz2);
26554 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26555 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26556 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26557 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26558 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26559 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26560 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26561 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26562 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26563 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26564 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26565 final double[] bias1 = calibrator.getInitialBias();
26566 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26567 final double[] bias2 = new double[3];
26568 calibrator.getInitialBias(bias2);
26569 assertArrayEquals(bias1, bias2, 0.0);
26570 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26571 assertEquals(b1, ba);
26572 final Matrix b2 = new Matrix(3, 1);
26573 calibrator.getInitialBiasAsMatrix(b2);
26574 assertEquals(b1, b2);
26575 final Matrix ma1 = new Matrix(3, 3);
26576 ma1.setSubmatrix(0, 0,
26577 2, 2,
26578 new double[]{sx, myx, mzx,
26579 mxy, sy, mzy,
26580 mxz, myz, sz});
26581 assertEquals(calibrator.getInitialMa(), ma1);
26582 final Matrix ma2 = new Matrix(3, 3);
26583 calibrator.getInitialMa(ma2);
26584 assertEquals(ma1, ma2);
26585 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26586 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26587 final NEDPosition nedPosition1 = new NEDPosition();
26588 assertTrue(calibrator.getNedPosition(nedPosition1));
26589 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26590 assertSame(calibrator.getMeasurements(), measurements);
26591 assertFalse(calibrator.isCommonAxisUsed());
26592 assertSame(calibrator.getListener(), this);
26593 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26594 assertFalse(calibrator.isReady());
26595 assertFalse(calibrator.isRunning());
26596 assertNull(calibrator.getEstimatedBiases());
26597 assertFalse(calibrator.getEstimatedBiases(null));
26598 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26599 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26600 assertNull(calibrator.getEstimatedBiasFx());
26601 assertNull(calibrator.getEstimatedBiasFy());
26602 assertNull(calibrator.getEstimatedBiasFz());
26603 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26604 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26605 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26606 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26607 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26608 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26609 assertNull(calibrator.getEstimatedMa());
26610 assertNull(calibrator.getEstimatedSx());
26611 assertNull(calibrator.getEstimatedSy());
26612 assertNull(calibrator.getEstimatedSz());
26613 assertNull(calibrator.getEstimatedMxy());
26614 assertNull(calibrator.getEstimatedMxz());
26615 assertNull(calibrator.getEstimatedMyx());
26616 assertNull(calibrator.getEstimatedMyz());
26617 assertNull(calibrator.getEstimatedMzx());
26618 assertNull(calibrator.getEstimatedMzy());
26619 assertNull(calibrator.getEstimatedCovariance());
26620 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26621 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26622 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26623 assertNotNull(calibrator.getGroundTruthGravityNorm());
26624 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26625 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26626 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26627 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26628 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26629 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26630 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26631 }
26632
26633 @Test
26634 public void testConstructor207() throws WrongSizeException {
26635 final Matrix ba = generateBa();
26636 final double biasX = ba.getElementAtIndex(0);
26637 final double biasY = ba.getElementAtIndex(1);
26638 final double biasZ = ba.getElementAtIndex(2);
26639
26640 final Acceleration bx = new Acceleration(biasX,
26641 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26642 final Acceleration by = new Acceleration(biasY,
26643 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26644 final Acceleration bz = new Acceleration(biasZ,
26645 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26646
26647 final Matrix ma = generateMaCommonAxis();
26648 final double sx = ma.getElementAt(0, 0);
26649 final double sy = ma.getElementAt(1, 1);
26650 final double sz = ma.getElementAt(2, 2);
26651 final double mxy = ma.getElementAt(0, 1);
26652 final double mxz = ma.getElementAt(0, 2);
26653 final double myx = ma.getElementAt(1, 0);
26654 final double myz = ma.getElementAt(1, 2);
26655 final double mzx = ma.getElementAt(2, 0);
26656 final double mzy = ma.getElementAt(2, 1);
26657
26658 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26659 final double latitude = Math.toRadians(
26660 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26661 final double longitude = Math.toRadians(
26662 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26663 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26664 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26665 final NEDVelocity nedVelocity = new NEDVelocity();
26666 final ECEFPosition ecefPosition = new ECEFPosition();
26667 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26668 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26669 ecefPosition, ecefVelocity);
26670
26671 final KnownPositionAccelerometerCalibrator calibrator =
26672 new KnownPositionAccelerometerCalibrator(nedPosition,
26673 true, bx, by, bz, sx, sy, sz, mxy, mxz,
26674 myx, myz, mzx, mzy);
26675
26676
26677 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26678 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26679 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26680 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26681 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26682 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26683 final Acceleration bx2 = new Acceleration(0.0,
26684 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26685 calibrator.getInitialBiasXAsAcceleration(bx2);
26686 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26687 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26688 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26689 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26690 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26691 final Acceleration by2 = new Acceleration(0.0,
26692 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26693 calibrator.getInitialBiasYAsAcceleration(by2);
26694 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26695 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26696 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26697 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26698 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26699 final Acceleration bz2 = new Acceleration(0.0,
26700 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26701 calibrator.getInitialBiasZAsAcceleration(bz2);
26702 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26703 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26704 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26705 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26706 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26707 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26708 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26709 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26710 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26711 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26712 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26713 final double[] bias1 = calibrator.getInitialBias();
26714 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26715 final double[] bias2 = new double[3];
26716 calibrator.getInitialBias(bias2);
26717 assertArrayEquals(bias1, bias2, 0.0);
26718 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26719 assertEquals(b1, ba);
26720 final Matrix b2 = new Matrix(3, 1);
26721 calibrator.getInitialBiasAsMatrix(b2);
26722 assertEquals(b1, b2);
26723 final Matrix ma1 = new Matrix(3, 3);
26724 ma1.setSubmatrix(0, 0,
26725 2, 2,
26726 new double[]{sx, myx, mzx,
26727 mxy, sy, mzy,
26728 mxz, myz, sz});
26729 assertEquals(calibrator.getInitialMa(), ma1);
26730 final Matrix ma2 = new Matrix(3, 3);
26731 calibrator.getInitialMa(ma2);
26732 assertEquals(ma1, ma2);
26733 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26734 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26735 final NEDPosition nedPosition1 = new NEDPosition();
26736 assertTrue(calibrator.getNedPosition(nedPosition1));
26737 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26738 assertNull(calibrator.getMeasurements());
26739 assertTrue(calibrator.isCommonAxisUsed());
26740 assertNull(calibrator.getListener());
26741 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26742 assertFalse(calibrator.isReady());
26743 assertFalse(calibrator.isRunning());
26744 assertNull(calibrator.getEstimatedBiases());
26745 assertFalse(calibrator.getEstimatedBiases(null));
26746 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26747 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26748 assertNull(calibrator.getEstimatedBiasFx());
26749 assertNull(calibrator.getEstimatedBiasFy());
26750 assertNull(calibrator.getEstimatedBiasFz());
26751 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26752 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26753 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26754 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26755 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26756 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26757 assertNull(calibrator.getEstimatedMa());
26758 assertNull(calibrator.getEstimatedSx());
26759 assertNull(calibrator.getEstimatedSy());
26760 assertNull(calibrator.getEstimatedSz());
26761 assertNull(calibrator.getEstimatedMxy());
26762 assertNull(calibrator.getEstimatedMxz());
26763 assertNull(calibrator.getEstimatedMyx());
26764 assertNull(calibrator.getEstimatedMyz());
26765 assertNull(calibrator.getEstimatedMzx());
26766 assertNull(calibrator.getEstimatedMzy());
26767 assertNull(calibrator.getEstimatedCovariance());
26768 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26769 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26770 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26771 assertNotNull(calibrator.getGroundTruthGravityNorm());
26772 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26773 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26774 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26775 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26776 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26777 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26778 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26779 }
26780
26781 @Test
26782 public void testConstructor208() throws WrongSizeException {
26783 final Matrix ba = generateBa();
26784 final double biasX = ba.getElementAtIndex(0);
26785 final double biasY = ba.getElementAtIndex(1);
26786 final double biasZ = ba.getElementAtIndex(2);
26787
26788 final Acceleration bx = new Acceleration(biasX,
26789 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26790 final Acceleration by = new Acceleration(biasY,
26791 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26792 final Acceleration bz = new Acceleration(biasZ,
26793 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26794
26795 final Matrix ma = generateMaCommonAxis();
26796 final double sx = ma.getElementAt(0, 0);
26797 final double sy = ma.getElementAt(1, 1);
26798 final double sz = ma.getElementAt(2, 2);
26799 final double mxy = ma.getElementAt(0, 1);
26800 final double mxz = ma.getElementAt(0, 2);
26801 final double myx = ma.getElementAt(1, 0);
26802 final double myz = ma.getElementAt(1, 2);
26803 final double mzx = ma.getElementAt(2, 0);
26804 final double mzy = ma.getElementAt(2, 1);
26805
26806 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26807 final double latitude = Math.toRadians(
26808 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26809 final double longitude = Math.toRadians(
26810 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26811 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26812 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26813 final NEDVelocity nedVelocity = new NEDVelocity();
26814 final ECEFPosition ecefPosition = new ECEFPosition();
26815 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26816 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26817 ecefPosition, ecefVelocity);
26818
26819 final KnownPositionAccelerometerCalibrator calibrator =
26820 new KnownPositionAccelerometerCalibrator(nedPosition,
26821 true, bx, by, bz, sx, sy, sz, mxy, mxz,
26822 myx, myz, mzx, mzy, this);
26823
26824
26825 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26826 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26827 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26828 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26829 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26830 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26831 final Acceleration bx2 = new Acceleration(0.0,
26832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26833 calibrator.getInitialBiasXAsAcceleration(bx2);
26834 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26835 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26836 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26837 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26838 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26839 final Acceleration by2 = new Acceleration(0.0,
26840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26841 calibrator.getInitialBiasYAsAcceleration(by2);
26842 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26843 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26844 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26845 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26846 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26847 final Acceleration bz2 = new Acceleration(0.0,
26848 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26849 calibrator.getInitialBiasZAsAcceleration(bz2);
26850 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26851 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26852 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26853 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26854 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26855 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26856 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26857 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26858 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26859 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26860 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26861 final double[] bias1 = calibrator.getInitialBias();
26862 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26863 final double[] bias2 = new double[3];
26864 calibrator.getInitialBias(bias2);
26865 assertArrayEquals(bias1, bias2, 0.0);
26866 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26867 assertEquals(b1, ba);
26868 final Matrix b2 = new Matrix(3, 1);
26869 calibrator.getInitialBiasAsMatrix(b2);
26870 assertEquals(b1, b2);
26871 final Matrix ma1 = new Matrix(3, 3);
26872 ma1.setSubmatrix(0, 0,
26873 2, 2,
26874 new double[]{sx, myx, mzx,
26875 mxy, sy, mzy,
26876 mxz, myz, sz});
26877 assertEquals(calibrator.getInitialMa(), ma1);
26878 final Matrix ma2 = new Matrix(3, 3);
26879 calibrator.getInitialMa(ma2);
26880 assertEquals(ma1, ma2);
26881 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26882 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26883 final NEDPosition nedPosition1 = new NEDPosition();
26884 assertTrue(calibrator.getNedPosition(nedPosition1));
26885 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26886 assertNull(calibrator.getMeasurements());
26887 assertTrue(calibrator.isCommonAxisUsed());
26888 assertSame(calibrator.getListener(), this);
26889 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26890 assertFalse(calibrator.isReady());
26891 assertFalse(calibrator.isRunning());
26892 assertNull(calibrator.getEstimatedBiases());
26893 assertFalse(calibrator.getEstimatedBiases(null));
26894 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26895 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26896 assertNull(calibrator.getEstimatedBiasFx());
26897 assertNull(calibrator.getEstimatedBiasFy());
26898 assertNull(calibrator.getEstimatedBiasFz());
26899 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26900 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26901 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26902 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26903 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26904 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26905 assertNull(calibrator.getEstimatedMa());
26906 assertNull(calibrator.getEstimatedSx());
26907 assertNull(calibrator.getEstimatedSy());
26908 assertNull(calibrator.getEstimatedSz());
26909 assertNull(calibrator.getEstimatedMxy());
26910 assertNull(calibrator.getEstimatedMxz());
26911 assertNull(calibrator.getEstimatedMyx());
26912 assertNull(calibrator.getEstimatedMyz());
26913 assertNull(calibrator.getEstimatedMzx());
26914 assertNull(calibrator.getEstimatedMzy());
26915 assertNull(calibrator.getEstimatedCovariance());
26916 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26917 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26918 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26919 assertNotNull(calibrator.getGroundTruthGravityNorm());
26920 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26921 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26922 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26923 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26924 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26925 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26926 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26927 }
26928
26929 @Test
26930 public void testConstructor209() throws WrongSizeException {
26931 final Collection<StandardDeviationBodyKinematics> measurements =
26932 Collections.emptyList();
26933
26934 final Matrix ba = generateBa();
26935 final double biasX = ba.getElementAtIndex(0);
26936 final double biasY = ba.getElementAtIndex(1);
26937 final double biasZ = ba.getElementAtIndex(2);
26938
26939 final Acceleration bx = new Acceleration(biasX,
26940 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26941 final Acceleration by = new Acceleration(biasY,
26942 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26943 final Acceleration bz = new Acceleration(biasZ,
26944 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26945
26946 final Matrix ma = generateMaCommonAxis();
26947 final double sx = ma.getElementAt(0, 0);
26948 final double sy = ma.getElementAt(1, 1);
26949 final double sz = ma.getElementAt(2, 2);
26950 final double mxy = ma.getElementAt(0, 1);
26951 final double mxz = ma.getElementAt(0, 2);
26952 final double myx = ma.getElementAt(1, 0);
26953 final double myz = ma.getElementAt(1, 2);
26954 final double mzx = ma.getElementAt(2, 0);
26955 final double mzy = ma.getElementAt(2, 1);
26956
26957 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26958 final double latitude = Math.toRadians(
26959 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26960 final double longitude = Math.toRadians(
26961 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26962 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26963 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26964 final NEDVelocity nedVelocity = new NEDVelocity();
26965 final ECEFPosition ecefPosition = new ECEFPosition();
26966 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26967 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26968 ecefPosition, ecefVelocity);
26969
26970 final KnownPositionAccelerometerCalibrator calibrator =
26971 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26972 true, bx, by, bz, sx, sy, sz, mxy, mxz,
26973 myx, myz, mzx, mzy);
26974
26975
26976 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26977 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26978 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26979 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26980 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26981 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26982 final Acceleration bx2 = new Acceleration(0.0,
26983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26984 calibrator.getInitialBiasXAsAcceleration(bx2);
26985 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26986 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26987 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26988 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26989 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26990 final Acceleration by2 = new Acceleration(0.0,
26991 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26992 calibrator.getInitialBiasYAsAcceleration(by2);
26993 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26994 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26995 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26996 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26997 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26998 final Acceleration bz2 = new Acceleration(0.0,
26999 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27000 calibrator.getInitialBiasZAsAcceleration(bz2);
27001 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27002 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27003 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27004 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27005 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27006 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27007 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27008 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27009 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27010 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27011 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27012 final double[] bias1 = calibrator.getInitialBias();
27013 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27014 final double[] bias2 = new double[3];
27015 calibrator.getInitialBias(bias2);
27016 assertArrayEquals(bias1, bias2, 0.0);
27017 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27018 assertEquals(b1, ba);
27019 final Matrix b2 = new Matrix(3, 1);
27020 calibrator.getInitialBiasAsMatrix(b2);
27021 assertEquals(b1, b2);
27022 final Matrix ma1 = new Matrix(3, 3);
27023 ma1.setSubmatrix(0, 0,
27024 2, 2,
27025 new double[]{sx, myx, mzx,
27026 mxy, sy, mzy,
27027 mxz, myz, sz});
27028 assertEquals(calibrator.getInitialMa(), ma1);
27029 final Matrix ma2 = new Matrix(3, 3);
27030 calibrator.getInitialMa(ma2);
27031 assertEquals(ma1, ma2);
27032 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27033 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27034 final NEDPosition nedPosition1 = new NEDPosition();
27035 assertTrue(calibrator.getNedPosition(nedPosition1));
27036 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27037 assertSame(calibrator.getMeasurements(), measurements);
27038 assertTrue(calibrator.isCommonAxisUsed());
27039 assertNull(calibrator.getListener());
27040 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27041 assertFalse(calibrator.isReady());
27042 assertFalse(calibrator.isRunning());
27043 assertNull(calibrator.getEstimatedBiases());
27044 assertFalse(calibrator.getEstimatedBiases(null));
27045 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27046 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27047 assertNull(calibrator.getEstimatedBiasFx());
27048 assertNull(calibrator.getEstimatedBiasFy());
27049 assertNull(calibrator.getEstimatedBiasFz());
27050 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27051 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27052 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27053 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27054 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27055 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27056 assertNull(calibrator.getEstimatedMa());
27057 assertNull(calibrator.getEstimatedSx());
27058 assertNull(calibrator.getEstimatedSy());
27059 assertNull(calibrator.getEstimatedSz());
27060 assertNull(calibrator.getEstimatedMxy());
27061 assertNull(calibrator.getEstimatedMxz());
27062 assertNull(calibrator.getEstimatedMyx());
27063 assertNull(calibrator.getEstimatedMyz());
27064 assertNull(calibrator.getEstimatedMzx());
27065 assertNull(calibrator.getEstimatedMzy());
27066 assertNull(calibrator.getEstimatedCovariance());
27067 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27068 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27069 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27070 assertNotNull(calibrator.getGroundTruthGravityNorm());
27071 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27072 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27073 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27074 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27075 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27076 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27077 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27078 }
27079
27080 @Test
27081 public void testConstructor210() throws WrongSizeException {
27082 final Collection<StandardDeviationBodyKinematics> measurements =
27083 Collections.emptyList();
27084
27085 final Matrix ba = generateBa();
27086 final double biasX = ba.getElementAtIndex(0);
27087 final double biasY = ba.getElementAtIndex(1);
27088 final double biasZ = ba.getElementAtIndex(2);
27089
27090 final Acceleration bx = new Acceleration(biasX,
27091 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27092 final Acceleration by = new Acceleration(biasY,
27093 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27094 final Acceleration bz = new Acceleration(biasZ,
27095 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27096
27097 final Matrix ma = generateMaCommonAxis();
27098 final double sx = ma.getElementAt(0, 0);
27099 final double sy = ma.getElementAt(1, 1);
27100 final double sz = ma.getElementAt(2, 2);
27101 final double mxy = ma.getElementAt(0, 1);
27102 final double mxz = ma.getElementAt(0, 2);
27103 final double myx = ma.getElementAt(1, 0);
27104 final double myz = ma.getElementAt(1, 2);
27105 final double mzx = ma.getElementAt(2, 0);
27106 final double mzy = ma.getElementAt(2, 1);
27107
27108 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27109 final double latitude = Math.toRadians(
27110 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27111 final double longitude = Math.toRadians(
27112 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27113 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27114 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27115 final NEDVelocity nedVelocity = new NEDVelocity();
27116 final ECEFPosition ecefPosition = new ECEFPosition();
27117 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27118 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27119 ecefPosition, ecefVelocity);
27120
27121 final KnownPositionAccelerometerCalibrator calibrator =
27122 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27123 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27124 myx, myz, mzx, mzy, this);
27125
27126
27127 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27128 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27129 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27130 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27131 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27132 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27133 final Acceleration bx2 = new Acceleration(0.0,
27134 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27135 calibrator.getInitialBiasXAsAcceleration(bx2);
27136 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27137 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27138 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27139 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27140 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27141 final Acceleration by2 = new Acceleration(0.0,
27142 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27143 calibrator.getInitialBiasYAsAcceleration(by2);
27144 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27145 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27146 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27147 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27148 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27149 final Acceleration bz2 = new Acceleration(0.0,
27150 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27151 calibrator.getInitialBiasZAsAcceleration(bz2);
27152 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27153 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27154 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27155 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27156 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27157 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27158 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27159 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27160 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27161 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27162 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27163 final double[] bias1 = calibrator.getInitialBias();
27164 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27165 final double[] bias2 = new double[3];
27166 calibrator.getInitialBias(bias2);
27167 assertArrayEquals(bias1, bias2, 0.0);
27168 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27169 assertEquals(b1, ba);
27170 final Matrix b2 = new Matrix(3, 1);
27171 calibrator.getInitialBiasAsMatrix(b2);
27172 assertEquals(b1, b2);
27173 final Matrix ma1 = new Matrix(3, 3);
27174 ma1.setSubmatrix(0, 0,
27175 2, 2,
27176 new double[]{sx, myx, mzx,
27177 mxy, sy, mzy,
27178 mxz, myz, sz});
27179 assertEquals(calibrator.getInitialMa(), ma1);
27180 final Matrix ma2 = new Matrix(3, 3);
27181 calibrator.getInitialMa(ma2);
27182 assertEquals(ma1, ma2);
27183 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27184 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27185 final NEDPosition nedPosition1 = new NEDPosition();
27186 assertTrue(calibrator.getNedPosition(nedPosition1));
27187 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27188 assertSame(calibrator.getMeasurements(), measurements);
27189 assertTrue(calibrator.isCommonAxisUsed());
27190 assertSame(calibrator.getListener(), this);
27191 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27192 assertFalse(calibrator.isReady());
27193 assertFalse(calibrator.isRunning());
27194 assertNull(calibrator.getEstimatedBiases());
27195 assertFalse(calibrator.getEstimatedBiases(null));
27196 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27197 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27198 assertNull(calibrator.getEstimatedBiasFx());
27199 assertNull(calibrator.getEstimatedBiasFy());
27200 assertNull(calibrator.getEstimatedBiasFz());
27201 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27202 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27203 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27204 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27205 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27206 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27207 assertNull(calibrator.getEstimatedMa());
27208 assertNull(calibrator.getEstimatedSx());
27209 assertNull(calibrator.getEstimatedSy());
27210 assertNull(calibrator.getEstimatedSz());
27211 assertNull(calibrator.getEstimatedMxy());
27212 assertNull(calibrator.getEstimatedMxz());
27213 assertNull(calibrator.getEstimatedMyx());
27214 assertNull(calibrator.getEstimatedMyz());
27215 assertNull(calibrator.getEstimatedMzx());
27216 assertNull(calibrator.getEstimatedMzy());
27217 assertNull(calibrator.getEstimatedCovariance());
27218 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27219 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27220 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27221 assertNotNull(calibrator.getGroundTruthGravityNorm());
27222 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27223 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27224 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27225 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27226 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27227 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27228 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27229 }
27230
27231 @Test
27232 public void testConstructor211() throws WrongSizeException {
27233 final Matrix ba = generateBa();
27234 final double[] bias = ba.getBuffer();
27235 final double biasX = ba.getElementAtIndex(0);
27236 final double biasY = ba.getElementAtIndex(1);
27237 final double biasZ = ba.getElementAtIndex(2);
27238
27239 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27240 final double latitude = Math.toRadians(
27241 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27242 final double longitude = Math.toRadians(
27243 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27244 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27245 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27246 final NEDVelocity nedVelocity = new NEDVelocity();
27247 final ECEFPosition ecefPosition = new ECEFPosition();
27248 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27249 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27250 ecefPosition, ecefVelocity);
27251
27252 KnownPositionAccelerometerCalibrator calibrator =
27253 new KnownPositionAccelerometerCalibrator(nedPosition, bias);
27254
27255
27256 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27257 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27258 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27259 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27260 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27261 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27262 final Acceleration bx2 = new Acceleration(0.0,
27263 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27264 calibrator.getInitialBiasXAsAcceleration(bx2);
27265 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27266 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27267 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27268 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27269 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27270 final Acceleration by2 = new Acceleration(0.0,
27271 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27272 calibrator.getInitialBiasYAsAcceleration(by2);
27273 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27274 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27275 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27276 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27277 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27278 final Acceleration bz2 = new Acceleration(0.0,
27279 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27280 calibrator.getInitialBiasZAsAcceleration(bz2);
27281 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27282 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27283 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27284 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27285 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27286 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27287 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27288 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27289 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27290 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27291 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27292 final double[] bias1 = calibrator.getInitialBias();
27293 assertArrayEquals(bias1, bias, 0.0);
27294 final double[] bias2 = new double[3];
27295 calibrator.getInitialBias(bias2);
27296 assertArrayEquals(bias1, bias2, 0.0);
27297 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27298 assertEquals(b1, ba);
27299 final Matrix b2 = new Matrix(3, 1);
27300 calibrator.getInitialBiasAsMatrix(b2);
27301 assertEquals(b1, b2);
27302 final Matrix ma1 = calibrator.getInitialMa();
27303 assertEquals(ma1, new Matrix(3, 3));
27304 final Matrix ma2 = new Matrix(3, 3);
27305 calibrator.getInitialMa(ma2);
27306 assertEquals(ma1, ma2);
27307 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27308 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27309 final NEDPosition nedPosition1 = new NEDPosition();
27310 assertTrue(calibrator.getNedPosition(nedPosition1));
27311 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27312 assertNull(calibrator.getMeasurements());
27313 assertFalse(calibrator.isCommonAxisUsed());
27314 assertNull(calibrator.getListener());
27315 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27316 assertFalse(calibrator.isReady());
27317 assertFalse(calibrator.isRunning());
27318 assertNull(calibrator.getEstimatedBiases());
27319 assertFalse(calibrator.getEstimatedBiases(null));
27320 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27321 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27322 assertNull(calibrator.getEstimatedBiasFx());
27323 assertNull(calibrator.getEstimatedBiasFy());
27324 assertNull(calibrator.getEstimatedBiasFz());
27325 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27326 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27327 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27328 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27329 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27330 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27331 assertNull(calibrator.getEstimatedMa());
27332 assertNull(calibrator.getEstimatedSx());
27333 assertNull(calibrator.getEstimatedSy());
27334 assertNull(calibrator.getEstimatedSz());
27335 assertNull(calibrator.getEstimatedMxy());
27336 assertNull(calibrator.getEstimatedMxz());
27337 assertNull(calibrator.getEstimatedMyx());
27338 assertNull(calibrator.getEstimatedMyz());
27339 assertNull(calibrator.getEstimatedMzx());
27340 assertNull(calibrator.getEstimatedMzy());
27341 assertNull(calibrator.getEstimatedCovariance());
27342 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27343 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27344 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27345 assertNotNull(calibrator.getGroundTruthGravityNorm());
27346 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27347 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27348 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27349 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27350 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27351 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27352 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27353
27354
27355 calibrator = null;
27356 try {
27357 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27358 new double[1]);
27359 fail("IllegalArgumentException expected but not thrown");
27360 } catch (final IllegalArgumentException ignore) {
27361 }
27362 assertNull(calibrator);
27363 }
27364
27365 @Test
27366 public void testConstructor212() throws WrongSizeException {
27367 final Matrix ba = generateBa();
27368 final double[] bias = ba.getBuffer();
27369 final double biasX = ba.getElementAtIndex(0);
27370 final double biasY = ba.getElementAtIndex(1);
27371 final double biasZ = ba.getElementAtIndex(2);
27372
27373 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27374 final double latitude = Math.toRadians(
27375 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27376 final double longitude = Math.toRadians(
27377 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27378 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27379 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27380 final NEDVelocity nedVelocity = new NEDVelocity();
27381 final ECEFPosition ecefPosition = new ECEFPosition();
27382 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27383 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27384 ecefPosition, ecefVelocity);
27385
27386 KnownPositionAccelerometerCalibrator calibrator =
27387 new KnownPositionAccelerometerCalibrator(nedPosition, bias,
27388 this);
27389
27390
27391 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27392 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27393 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27394 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27395 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27396 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27397 final Acceleration bx2 = new Acceleration(0.0,
27398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27399 calibrator.getInitialBiasXAsAcceleration(bx2);
27400 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27401 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27402 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27403 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27404 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27405 final Acceleration by2 = new Acceleration(0.0,
27406 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27407 calibrator.getInitialBiasYAsAcceleration(by2);
27408 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27409 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27410 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27411 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27412 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27413 final Acceleration bz2 = new Acceleration(0.0,
27414 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27415 calibrator.getInitialBiasZAsAcceleration(bz2);
27416 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27417 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27418 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27419 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27420 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27421 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27422 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27423 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27424 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27425 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27426 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27427 final double[] bias1 = calibrator.getInitialBias();
27428 assertArrayEquals(bias1, bias, 0.0);
27429 final double[] bias2 = new double[3];
27430 calibrator.getInitialBias(bias2);
27431 assertArrayEquals(bias1, bias2, 0.0);
27432 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27433 assertEquals(b1, ba);
27434 final Matrix b2 = new Matrix(3, 1);
27435 calibrator.getInitialBiasAsMatrix(b2);
27436 assertEquals(b1, b2);
27437 final Matrix ma1 = calibrator.getInitialMa();
27438 assertEquals(ma1, new Matrix(3, 3));
27439 final Matrix ma2 = new Matrix(3, 3);
27440 calibrator.getInitialMa(ma2);
27441 assertEquals(ma1, ma2);
27442 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27443 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27444 final NEDPosition nedPosition1 = new NEDPosition();
27445 assertTrue(calibrator.getNedPosition(nedPosition1));
27446 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27447 assertNull(calibrator.getMeasurements());
27448 assertFalse(calibrator.isCommonAxisUsed());
27449 assertSame(calibrator.getListener(), this);
27450 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27451 assertFalse(calibrator.isReady());
27452 assertFalse(calibrator.isRunning());
27453 assertNull(calibrator.getEstimatedBiases());
27454 assertFalse(calibrator.getEstimatedBiases(null));
27455 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27456 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27457 assertNull(calibrator.getEstimatedBiasFx());
27458 assertNull(calibrator.getEstimatedBiasFy());
27459 assertNull(calibrator.getEstimatedBiasFz());
27460 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27461 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27462 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27463 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27464 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27465 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27466 assertNull(calibrator.getEstimatedMa());
27467 assertNull(calibrator.getEstimatedSx());
27468 assertNull(calibrator.getEstimatedSy());
27469 assertNull(calibrator.getEstimatedSz());
27470 assertNull(calibrator.getEstimatedMxy());
27471 assertNull(calibrator.getEstimatedMxz());
27472 assertNull(calibrator.getEstimatedMyx());
27473 assertNull(calibrator.getEstimatedMyz());
27474 assertNull(calibrator.getEstimatedMzx());
27475 assertNull(calibrator.getEstimatedMzy());
27476 assertNull(calibrator.getEstimatedCovariance());
27477 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27478 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27479 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27480 assertNotNull(calibrator.getGroundTruthGravityNorm());
27481 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27482 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27483 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27484 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27485 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27486 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27487 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27488
27489
27490 calibrator = null;
27491 try {
27492 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27493 new double[1], this);
27494 fail("IllegalArgumentException expected but not thrown");
27495 } catch (final IllegalArgumentException ignore) {
27496 }
27497 assertNull(calibrator);
27498 }
27499
27500 @Test
27501 public void testConstructor213() throws WrongSizeException {
27502 final Collection<StandardDeviationBodyKinematics> measurements =
27503 Collections.emptyList();
27504
27505 final Matrix ba = generateBa();
27506 final double[] bias = ba.getBuffer();
27507 final double biasX = ba.getElementAtIndex(0);
27508 final double biasY = ba.getElementAtIndex(1);
27509 final double biasZ = ba.getElementAtIndex(2);
27510
27511 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27512 final double latitude = Math.toRadians(
27513 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27514 final double longitude = Math.toRadians(
27515 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27516 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27517 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27518 final NEDVelocity nedVelocity = new NEDVelocity();
27519 final ECEFPosition ecefPosition = new ECEFPosition();
27520 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27521 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27522 ecefPosition, ecefVelocity);
27523
27524 KnownPositionAccelerometerCalibrator calibrator =
27525 new KnownPositionAccelerometerCalibrator(nedPosition,
27526 measurements, bias);
27527
27528
27529 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27530 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27531 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27532 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27533 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27534 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27535 final Acceleration bx2 = new Acceleration(0.0,
27536 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27537 calibrator.getInitialBiasXAsAcceleration(bx2);
27538 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27539 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27540 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27541 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27542 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27543 final Acceleration by2 = new Acceleration(0.0,
27544 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27545 calibrator.getInitialBiasYAsAcceleration(by2);
27546 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27547 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27548 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27549 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27550 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27551 final Acceleration bz2 = new Acceleration(0.0,
27552 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27553 calibrator.getInitialBiasZAsAcceleration(bz2);
27554 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27555 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27556 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27557 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27558 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27559 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27560 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27561 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27562 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27563 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27564 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27565 final double[] bias1 = calibrator.getInitialBias();
27566 assertArrayEquals(bias1, bias, 0.0);
27567 final double[] bias2 = new double[3];
27568 calibrator.getInitialBias(bias2);
27569 assertArrayEquals(bias1, bias2, 0.0);
27570 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27571 assertEquals(b1, ba);
27572 final Matrix b2 = new Matrix(3, 1);
27573 calibrator.getInitialBiasAsMatrix(b2);
27574 assertEquals(b1, b2);
27575 final Matrix ma1 = calibrator.getInitialMa();
27576 assertEquals(ma1, new Matrix(3, 3));
27577 final Matrix ma2 = new Matrix(3, 3);
27578 calibrator.getInitialMa(ma2);
27579 assertEquals(ma1, ma2);
27580 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27581 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27582 final NEDPosition nedPosition1 = new NEDPosition();
27583 assertTrue(calibrator.getNedPosition(nedPosition1));
27584 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27585 assertSame(calibrator.getMeasurements(), measurements);
27586 assertFalse(calibrator.isCommonAxisUsed());
27587 assertNull(calibrator.getListener());
27588 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27589 assertFalse(calibrator.isReady());
27590 assertFalse(calibrator.isRunning());
27591 assertNull(calibrator.getEstimatedBiases());
27592 assertFalse(calibrator.getEstimatedBiases(null));
27593 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27594 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27595 assertNull(calibrator.getEstimatedBiasFx());
27596 assertNull(calibrator.getEstimatedBiasFy());
27597 assertNull(calibrator.getEstimatedBiasFz());
27598 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27599 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27600 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27601 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27602 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27603 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27604 assertNull(calibrator.getEstimatedMa());
27605 assertNull(calibrator.getEstimatedSx());
27606 assertNull(calibrator.getEstimatedSy());
27607 assertNull(calibrator.getEstimatedSz());
27608 assertNull(calibrator.getEstimatedMxy());
27609 assertNull(calibrator.getEstimatedMxz());
27610 assertNull(calibrator.getEstimatedMyx());
27611 assertNull(calibrator.getEstimatedMyz());
27612 assertNull(calibrator.getEstimatedMzx());
27613 assertNull(calibrator.getEstimatedMzy());
27614 assertNull(calibrator.getEstimatedCovariance());
27615 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27616 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27617 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27618 assertNotNull(calibrator.getGroundTruthGravityNorm());
27619 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27620 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27621 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27622 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27623 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27624 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27625 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27626
27627
27628 calibrator = null;
27629 try {
27630 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27631 measurements, new double[1]);
27632 fail("IllegalArgumentException expected but not thrown");
27633 } catch (final IllegalArgumentException ignore) {
27634 }
27635 assertNull(calibrator);
27636 }
27637
27638 @Test
27639 public void testConstructor214() throws WrongSizeException {
27640 final Collection<StandardDeviationBodyKinematics> measurements =
27641 Collections.emptyList();
27642
27643 final Matrix ba = generateBa();
27644 final double[] bias = ba.getBuffer();
27645 final double biasX = ba.getElementAtIndex(0);
27646 final double biasY = ba.getElementAtIndex(1);
27647 final double biasZ = ba.getElementAtIndex(2);
27648
27649 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27650 final double latitude = Math.toRadians(
27651 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27652 final double longitude = Math.toRadians(
27653 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27654 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27655 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27656 final NEDVelocity nedVelocity = new NEDVelocity();
27657 final ECEFPosition ecefPosition = new ECEFPosition();
27658 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27659 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27660 ecefPosition, ecefVelocity);
27661
27662 KnownPositionAccelerometerCalibrator calibrator =
27663 new KnownPositionAccelerometerCalibrator(nedPosition,
27664 measurements, bias, this);
27665
27666
27667 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27668 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27669 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27670 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27671 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27672 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27673 final Acceleration bx2 = new Acceleration(0.0,
27674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27675 calibrator.getInitialBiasXAsAcceleration(bx2);
27676 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27677 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27678 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27679 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27680 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27681 final Acceleration by2 = new Acceleration(0.0,
27682 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27683 calibrator.getInitialBiasYAsAcceleration(by2);
27684 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27685 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27686 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27687 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27688 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27689 final Acceleration bz2 = new Acceleration(0.0,
27690 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27691 calibrator.getInitialBiasZAsAcceleration(bz2);
27692 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27693 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27694 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27695 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27696 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27697 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27698 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27699 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27700 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27701 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27702 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27703 final double[] bias1 = calibrator.getInitialBias();
27704 assertArrayEquals(bias1, bias, 0.0);
27705 final double[] bias2 = new double[3];
27706 calibrator.getInitialBias(bias2);
27707 assertArrayEquals(bias1, bias2, 0.0);
27708 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27709 assertEquals(b1, ba);
27710 final Matrix b2 = new Matrix(3, 1);
27711 calibrator.getInitialBiasAsMatrix(b2);
27712 assertEquals(b1, b2);
27713 final Matrix ma1 = calibrator.getInitialMa();
27714 assertEquals(ma1, new Matrix(3, 3));
27715 final Matrix ma2 = new Matrix(3, 3);
27716 calibrator.getInitialMa(ma2);
27717 assertEquals(ma1, ma2);
27718 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27719 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27720 final NEDPosition nedPosition1 = new NEDPosition();
27721 assertTrue(calibrator.getNedPosition(nedPosition1));
27722 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27723 assertSame(calibrator.getMeasurements(), measurements);
27724 assertFalse(calibrator.isCommonAxisUsed());
27725 assertSame(calibrator.getListener(), this);
27726 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27727 assertFalse(calibrator.isReady());
27728 assertFalse(calibrator.isRunning());
27729 assertNull(calibrator.getEstimatedBiases());
27730 assertFalse(calibrator.getEstimatedBiases(null));
27731 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27732 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27733 assertNull(calibrator.getEstimatedBiasFx());
27734 assertNull(calibrator.getEstimatedBiasFy());
27735 assertNull(calibrator.getEstimatedBiasFz());
27736 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27737 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27738 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27739 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27740 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27741 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27742 assertNull(calibrator.getEstimatedMa());
27743 assertNull(calibrator.getEstimatedSx());
27744 assertNull(calibrator.getEstimatedSy());
27745 assertNull(calibrator.getEstimatedSz());
27746 assertNull(calibrator.getEstimatedMxy());
27747 assertNull(calibrator.getEstimatedMxz());
27748 assertNull(calibrator.getEstimatedMyx());
27749 assertNull(calibrator.getEstimatedMyz());
27750 assertNull(calibrator.getEstimatedMzx());
27751 assertNull(calibrator.getEstimatedMzy());
27752 assertNull(calibrator.getEstimatedCovariance());
27753 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27754 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27755 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27756 assertNotNull(calibrator.getGroundTruthGravityNorm());
27757 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27758 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27759 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27760 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27761 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27762 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27763 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27764
27765
27766 calibrator = null;
27767 try {
27768 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27769 measurements, new double[1], this);
27770 fail("IllegalArgumentException expected but not thrown");
27771 } catch (final IllegalArgumentException ignore) {
27772 }
27773 assertNull(calibrator);
27774 }
27775
27776 @Test
27777 public void testConstructor215() throws WrongSizeException {
27778 final Matrix ba = generateBa();
27779 final double[] bias = ba.getBuffer();
27780 final double biasX = ba.getElementAtIndex(0);
27781 final double biasY = ba.getElementAtIndex(1);
27782 final double biasZ = ba.getElementAtIndex(2);
27783
27784 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27785 final double latitude = Math.toRadians(
27786 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27787 final double longitude = Math.toRadians(
27788 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27789 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27790 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27791 final NEDVelocity nedVelocity = new NEDVelocity();
27792 final ECEFPosition ecefPosition = new ECEFPosition();
27793 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27794 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27795 ecefPosition, ecefVelocity);
27796
27797 KnownPositionAccelerometerCalibrator calibrator =
27798 new KnownPositionAccelerometerCalibrator(nedPosition,
27799 true, bias);
27800
27801
27802 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27803 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27804 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27805 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27806 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27807 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27808 final Acceleration bx2 = new Acceleration(0.0,
27809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27810 calibrator.getInitialBiasXAsAcceleration(bx2);
27811 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27812 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27813 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27814 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27815 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27816 final Acceleration by2 = new Acceleration(0.0,
27817 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27818 calibrator.getInitialBiasYAsAcceleration(by2);
27819 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27820 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27821 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27822 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27823 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27824 final Acceleration bz2 = new Acceleration(0.0,
27825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27826 calibrator.getInitialBiasZAsAcceleration(bz2);
27827 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27828 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27829 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27830 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27831 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27832 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27833 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27834 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27835 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27836 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27837 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27838 final double[] bias1 = calibrator.getInitialBias();
27839 assertArrayEquals(bias1, bias, 0.0);
27840 final double[] bias2 = new double[3];
27841 calibrator.getInitialBias(bias2);
27842 assertArrayEquals(bias1, bias2, 0.0);
27843 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27844 assertEquals(b1, ba);
27845 final Matrix b2 = new Matrix(3, 1);
27846 calibrator.getInitialBiasAsMatrix(b2);
27847 assertEquals(b1, b2);
27848 final Matrix ma1 = calibrator.getInitialMa();
27849 assertEquals(ma1, new Matrix(3, 3));
27850 final Matrix ma2 = new Matrix(3, 3);
27851 calibrator.getInitialMa(ma2);
27852 assertEquals(ma1, ma2);
27853 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27854 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27855 final NEDPosition nedPosition1 = new NEDPosition();
27856 assertTrue(calibrator.getNedPosition(nedPosition1));
27857 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27858 assertNull(calibrator.getMeasurements());
27859 assertTrue(calibrator.isCommonAxisUsed());
27860 assertNull(calibrator.getListener());
27861 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27862 assertFalse(calibrator.isReady());
27863 assertFalse(calibrator.isRunning());
27864 assertNull(calibrator.getEstimatedBiases());
27865 assertFalse(calibrator.getEstimatedBiases(null));
27866 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27867 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27868 assertNull(calibrator.getEstimatedBiasFx());
27869 assertNull(calibrator.getEstimatedBiasFy());
27870 assertNull(calibrator.getEstimatedBiasFz());
27871 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27872 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27873 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27874 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27875 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27876 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27877 assertNull(calibrator.getEstimatedMa());
27878 assertNull(calibrator.getEstimatedSx());
27879 assertNull(calibrator.getEstimatedSy());
27880 assertNull(calibrator.getEstimatedSz());
27881 assertNull(calibrator.getEstimatedMxy());
27882 assertNull(calibrator.getEstimatedMxz());
27883 assertNull(calibrator.getEstimatedMyx());
27884 assertNull(calibrator.getEstimatedMyz());
27885 assertNull(calibrator.getEstimatedMzx());
27886 assertNull(calibrator.getEstimatedMzy());
27887 assertNull(calibrator.getEstimatedCovariance());
27888 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27889 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27890 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27891 assertNotNull(calibrator.getGroundTruthGravityNorm());
27892 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27893 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27894 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27895 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27896 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27897 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27898 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27899
27900
27901 calibrator = null;
27902 try {
27903 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27904 true, new double[1]);
27905 fail("IllegalArgumentException expected but not thrown");
27906 } catch (final IllegalArgumentException ignore) {
27907 }
27908 assertNull(calibrator);
27909 }
27910
27911 @Test
27912 public void testConstructor216() throws WrongSizeException {
27913 final Matrix ba = generateBa();
27914 final double[] bias = ba.getBuffer();
27915 final double biasX = ba.getElementAtIndex(0);
27916 final double biasY = ba.getElementAtIndex(1);
27917 final double biasZ = ba.getElementAtIndex(2);
27918
27919 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27920 final double latitude = Math.toRadians(
27921 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27922 final double longitude = Math.toRadians(
27923 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27924 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27925 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27926 final NEDVelocity nedVelocity = new NEDVelocity();
27927 final ECEFPosition ecefPosition = new ECEFPosition();
27928 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27929 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27930 ecefPosition, ecefVelocity);
27931
27932 KnownPositionAccelerometerCalibrator calibrator =
27933 new KnownPositionAccelerometerCalibrator(nedPosition,
27934 true, bias, this);
27935
27936
27937 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27938 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27939 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27940 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27941 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27942 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27943 final Acceleration bx2 = new Acceleration(0.0,
27944 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27945 calibrator.getInitialBiasXAsAcceleration(bx2);
27946 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27947 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27948 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27949 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27950 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27951 final Acceleration by2 = new Acceleration(0.0,
27952 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27953 calibrator.getInitialBiasYAsAcceleration(by2);
27954 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27955 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27956 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27957 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27958 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27959 final Acceleration bz2 = new Acceleration(0.0,
27960 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27961 calibrator.getInitialBiasZAsAcceleration(bz2);
27962 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27963 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27964 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27965 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27966 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27967 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27968 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27969 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27970 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27971 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27972 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27973 final double[] bias1 = calibrator.getInitialBias();
27974 assertArrayEquals(bias1, bias, 0.0);
27975 final double[] bias2 = new double[3];
27976 calibrator.getInitialBias(bias2);
27977 assertArrayEquals(bias1, bias2, 0.0);
27978 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27979 assertEquals(b1, ba);
27980 final Matrix b2 = new Matrix(3, 1);
27981 calibrator.getInitialBiasAsMatrix(b2);
27982 assertEquals(b1, b2);
27983 final Matrix ma1 = calibrator.getInitialMa();
27984 assertEquals(ma1, new Matrix(3, 3));
27985 final Matrix ma2 = new Matrix(3, 3);
27986 calibrator.getInitialMa(ma2);
27987 assertEquals(ma1, ma2);
27988 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27989 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27990 final NEDPosition nedPosition1 = new NEDPosition();
27991 assertTrue(calibrator.getNedPosition(nedPosition1));
27992 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27993 assertNull(calibrator.getMeasurements());
27994 assertTrue(calibrator.isCommonAxisUsed());
27995 assertSame(calibrator.getListener(), this);
27996 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27997 assertFalse(calibrator.isReady());
27998 assertFalse(calibrator.isRunning());
27999 assertNull(calibrator.getEstimatedBiases());
28000 assertFalse(calibrator.getEstimatedBiases(null));
28001 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28002 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28003 assertNull(calibrator.getEstimatedBiasFx());
28004 assertNull(calibrator.getEstimatedBiasFy());
28005 assertNull(calibrator.getEstimatedBiasFz());
28006 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28007 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28008 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28009 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28010 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28011 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28012 assertNull(calibrator.getEstimatedMa());
28013 assertNull(calibrator.getEstimatedSx());
28014 assertNull(calibrator.getEstimatedSy());
28015 assertNull(calibrator.getEstimatedSz());
28016 assertNull(calibrator.getEstimatedMxy());
28017 assertNull(calibrator.getEstimatedMxz());
28018 assertNull(calibrator.getEstimatedMyx());
28019 assertNull(calibrator.getEstimatedMyz());
28020 assertNull(calibrator.getEstimatedMzx());
28021 assertNull(calibrator.getEstimatedMzy());
28022 assertNull(calibrator.getEstimatedCovariance());
28023 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28024 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28025 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28026 assertNotNull(calibrator.getGroundTruthGravityNorm());
28027 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28028 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28029 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28030 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28031 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28032 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28033 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28034
28035
28036 calibrator = null;
28037 try {
28038 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28039 true, new double[1], this);
28040 fail("IllegalArgumentException expected but not thrown");
28041 } catch (final IllegalArgumentException ignore) {
28042 }
28043 assertNull(calibrator);
28044 }
28045
28046 @Test
28047 public void testConstructor217() throws WrongSizeException {
28048 final Collection<StandardDeviationBodyKinematics> measurements =
28049 Collections.emptyList();
28050
28051 final Matrix ba = generateBa();
28052 final double[] bias = ba.getBuffer();
28053 final double biasX = ba.getElementAtIndex(0);
28054 final double biasY = ba.getElementAtIndex(1);
28055 final double biasZ = ba.getElementAtIndex(2);
28056
28057 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28058 final double latitude = Math.toRadians(
28059 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28060 final double longitude = Math.toRadians(
28061 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28062 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28063 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28064 final NEDVelocity nedVelocity = new NEDVelocity();
28065 final ECEFPosition ecefPosition = new ECEFPosition();
28066 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28067 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28068 ecefPosition, ecefVelocity);
28069
28070 KnownPositionAccelerometerCalibrator calibrator =
28071 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28072 true, bias);
28073
28074
28075 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28076 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28077 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28078 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28079 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28080 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28081 final Acceleration bx2 = new Acceleration(0.0,
28082 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28083 calibrator.getInitialBiasXAsAcceleration(bx2);
28084 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28085 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28086 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28087 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28088 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28089 final Acceleration by2 = new Acceleration(0.0,
28090 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28091 calibrator.getInitialBiasYAsAcceleration(by2);
28092 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28093 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28094 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28095 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28096 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28097 final Acceleration bz2 = new Acceleration(0.0,
28098 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28099 calibrator.getInitialBiasZAsAcceleration(bz2);
28100 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28101 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28102 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28103 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28104 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28105 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28106 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28107 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28108 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28109 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28110 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28111 final double[] bias1 = calibrator.getInitialBias();
28112 assertArrayEquals(bias1, bias, 0.0);
28113 final double[] bias2 = new double[3];
28114 calibrator.getInitialBias(bias2);
28115 assertArrayEquals(bias1, bias2, 0.0);
28116 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28117 assertEquals(b1, ba);
28118 final Matrix b2 = new Matrix(3, 1);
28119 calibrator.getInitialBiasAsMatrix(b2);
28120 assertEquals(b1, b2);
28121 final Matrix ma1 = calibrator.getInitialMa();
28122 assertEquals(ma1, new Matrix(3, 3));
28123 final Matrix ma2 = new Matrix(3, 3);
28124 calibrator.getInitialMa(ma2);
28125 assertEquals(ma1, ma2);
28126 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28127 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28128 final NEDPosition nedPosition1 = new NEDPosition();
28129 assertTrue(calibrator.getNedPosition(nedPosition1));
28130 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28131 assertSame(calibrator.getMeasurements(), measurements);
28132 assertTrue(calibrator.isCommonAxisUsed());
28133 assertNull(calibrator.getListener());
28134 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28135 assertFalse(calibrator.isReady());
28136 assertFalse(calibrator.isRunning());
28137 assertNull(calibrator.getEstimatedBiases());
28138 assertFalse(calibrator.getEstimatedBiases(null));
28139 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28140 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28141 assertNull(calibrator.getEstimatedBiasFx());
28142 assertNull(calibrator.getEstimatedBiasFy());
28143 assertNull(calibrator.getEstimatedBiasFz());
28144 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28145 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28146 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28147 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28148 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28149 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28150 assertNull(calibrator.getEstimatedMa());
28151 assertNull(calibrator.getEstimatedSx());
28152 assertNull(calibrator.getEstimatedSy());
28153 assertNull(calibrator.getEstimatedSz());
28154 assertNull(calibrator.getEstimatedMxy());
28155 assertNull(calibrator.getEstimatedMxz());
28156 assertNull(calibrator.getEstimatedMyx());
28157 assertNull(calibrator.getEstimatedMyz());
28158 assertNull(calibrator.getEstimatedMzx());
28159 assertNull(calibrator.getEstimatedMzy());
28160 assertNull(calibrator.getEstimatedCovariance());
28161 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28162 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28163 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28164 assertNotNull(calibrator.getGroundTruthGravityNorm());
28165 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28166 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28167 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28168 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28169 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28170 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28171 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28172
28173
28174 calibrator = null;
28175 try {
28176 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28177 measurements, true, new double[1]);
28178 fail("IllegalArgumentException expected but not thrown");
28179 } catch (final IllegalArgumentException ignore) {
28180 }
28181 assertNull(calibrator);
28182 }
28183
28184 @Test
28185 public void testConstructor218() throws WrongSizeException {
28186 final Collection<StandardDeviationBodyKinematics> measurements =
28187 Collections.emptyList();
28188
28189 final Matrix ba = generateBa();
28190 final double[] bias = ba.getBuffer();
28191 final double biasX = ba.getElementAtIndex(0);
28192 final double biasY = ba.getElementAtIndex(1);
28193 final double biasZ = ba.getElementAtIndex(2);
28194
28195 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28196 final double latitude = Math.toRadians(
28197 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28198 final double longitude = Math.toRadians(
28199 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28200 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28201 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28202 final NEDVelocity nedVelocity = new NEDVelocity();
28203 final ECEFPosition ecefPosition = new ECEFPosition();
28204 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28205 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28206 ecefPosition, ecefVelocity);
28207
28208 KnownPositionAccelerometerCalibrator calibrator =
28209 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28210 true, bias, this);
28211
28212
28213 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28214 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28215 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28216 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28217 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28218 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28219 final Acceleration bx2 = new Acceleration(0.0,
28220 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28221 calibrator.getInitialBiasXAsAcceleration(bx2);
28222 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28223 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28224 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28225 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28226 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28227 final Acceleration by2 = new Acceleration(0.0,
28228 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28229 calibrator.getInitialBiasYAsAcceleration(by2);
28230 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28231 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28232 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28233 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28234 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28235 final Acceleration bz2 = new Acceleration(0.0,
28236 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28237 calibrator.getInitialBiasZAsAcceleration(bz2);
28238 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28239 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28240 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28241 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28242 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28243 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28244 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28245 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28246 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28247 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28248 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28249 final double[] bias1 = calibrator.getInitialBias();
28250 assertArrayEquals(bias1, bias, 0.0);
28251 final double[] bias2 = new double[3];
28252 calibrator.getInitialBias(bias2);
28253 assertArrayEquals(bias1, bias2, 0.0);
28254 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28255 assertEquals(b1, ba);
28256 final Matrix b2 = new Matrix(3, 1);
28257 calibrator.getInitialBiasAsMatrix(b2);
28258 assertEquals(b1, b2);
28259 final Matrix ma1 = calibrator.getInitialMa();
28260 assertEquals(ma1, new Matrix(3, 3));
28261 final Matrix ma2 = new Matrix(3, 3);
28262 calibrator.getInitialMa(ma2);
28263 assertEquals(ma1, ma2);
28264 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28265 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28266 final NEDPosition nedPosition1 = new NEDPosition();
28267 assertTrue(calibrator.getNedPosition(nedPosition1));
28268 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28269 assertSame(calibrator.getMeasurements(), measurements);
28270 assertTrue(calibrator.isCommonAxisUsed());
28271 assertSame(calibrator.getListener(), this);
28272 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28273 assertFalse(calibrator.isReady());
28274 assertFalse(calibrator.isRunning());
28275 assertNull(calibrator.getEstimatedBiases());
28276 assertFalse(calibrator.getEstimatedBiases(null));
28277 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28278 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28279 assertNull(calibrator.getEstimatedBiasFx());
28280 assertNull(calibrator.getEstimatedBiasFy());
28281 assertNull(calibrator.getEstimatedBiasFz());
28282 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28283 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28284 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28285 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28286 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28287 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28288 assertNull(calibrator.getEstimatedMa());
28289 assertNull(calibrator.getEstimatedSx());
28290 assertNull(calibrator.getEstimatedSy());
28291 assertNull(calibrator.getEstimatedSz());
28292 assertNull(calibrator.getEstimatedMxy());
28293 assertNull(calibrator.getEstimatedMxz());
28294 assertNull(calibrator.getEstimatedMyx());
28295 assertNull(calibrator.getEstimatedMyz());
28296 assertNull(calibrator.getEstimatedMzx());
28297 assertNull(calibrator.getEstimatedMzy());
28298 assertNull(calibrator.getEstimatedCovariance());
28299 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28300 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28301 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28302 assertNotNull(calibrator.getGroundTruthGravityNorm());
28303 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28304 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28305 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28306 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28307 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28308 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28309 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28310
28311
28312 calibrator = null;
28313 try {
28314 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28315 measurements, true, new double[1], this);
28316 fail("IllegalArgumentException expected but not thrown");
28317 } catch (final IllegalArgumentException ignore) {
28318 }
28319 assertNull(calibrator);
28320 }
28321
28322 @Test
28323 public void testConstructor219() throws WrongSizeException {
28324 final Matrix ba = generateBa();
28325 final double[] bias = ba.getBuffer();
28326 final double biasX = ba.getElementAtIndex(0);
28327 final double biasY = ba.getElementAtIndex(1);
28328 final double biasZ = ba.getElementAtIndex(2);
28329
28330 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28331 final double latitude = Math.toRadians(
28332 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28333 final double longitude = Math.toRadians(
28334 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28335 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28336 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28337 final NEDVelocity nedVelocity = new NEDVelocity();
28338 final ECEFPosition ecefPosition = new ECEFPosition();
28339 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28340 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28341 ecefPosition, ecefVelocity);
28342
28343 KnownPositionAccelerometerCalibrator calibrator =
28344 new KnownPositionAccelerometerCalibrator(nedPosition, ba);
28345
28346
28347 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28348 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28349 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28350 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28351 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28352 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28353 final Acceleration bx2 = new Acceleration(0.0,
28354 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28355 calibrator.getInitialBiasXAsAcceleration(bx2);
28356 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28357 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28358 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28359 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28360 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28361 final Acceleration by2 = new Acceleration(0.0,
28362 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28363 calibrator.getInitialBiasYAsAcceleration(by2);
28364 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28365 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28366 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28367 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28368 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28369 final Acceleration bz2 = new Acceleration(0.0,
28370 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28371 calibrator.getInitialBiasZAsAcceleration(bz2);
28372 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28373 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28374 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28375 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28376 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28377 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28378 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28379 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28380 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28381 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28382 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28383 final double[] bias1 = calibrator.getInitialBias();
28384 assertArrayEquals(bias1, bias, 0.0);
28385 final double[] bias2 = new double[3];
28386 calibrator.getInitialBias(bias2);
28387 assertArrayEquals(bias1, bias2, 0.0);
28388 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28389 assertEquals(b1, ba);
28390 final Matrix b2 = new Matrix(3, 1);
28391 calibrator.getInitialBiasAsMatrix(b2);
28392 assertEquals(b1, b2);
28393 final Matrix ma1 = calibrator.getInitialMa();
28394 assertEquals(ma1, new Matrix(3, 3));
28395 final Matrix ma2 = new Matrix(3, 3);
28396 calibrator.getInitialMa(ma2);
28397 assertEquals(ma1, ma2);
28398 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28399 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28400 final NEDPosition nedPosition1 = new NEDPosition();
28401 assertTrue(calibrator.getNedPosition(nedPosition1));
28402 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28403 assertNull(calibrator.getMeasurements());
28404 assertFalse(calibrator.isCommonAxisUsed());
28405 assertNull(calibrator.getListener());
28406 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28407 assertFalse(calibrator.isReady());
28408 assertFalse(calibrator.isRunning());
28409 assertNull(calibrator.getEstimatedBiases());
28410 assertFalse(calibrator.getEstimatedBiases(null));
28411 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28412 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28413 assertNull(calibrator.getEstimatedBiasFx());
28414 assertNull(calibrator.getEstimatedBiasFy());
28415 assertNull(calibrator.getEstimatedBiasFz());
28416 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28417 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28418 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28419 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28420 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28421 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28422 assertNull(calibrator.getEstimatedMa());
28423 assertNull(calibrator.getEstimatedSx());
28424 assertNull(calibrator.getEstimatedSy());
28425 assertNull(calibrator.getEstimatedSz());
28426 assertNull(calibrator.getEstimatedMxy());
28427 assertNull(calibrator.getEstimatedMxz());
28428 assertNull(calibrator.getEstimatedMyx());
28429 assertNull(calibrator.getEstimatedMyz());
28430 assertNull(calibrator.getEstimatedMzx());
28431 assertNull(calibrator.getEstimatedMzy());
28432 assertNull(calibrator.getEstimatedCovariance());
28433 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28434 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28435 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28436 assertNotNull(calibrator.getGroundTruthGravityNorm());
28437 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28438 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28439 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28440 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28441 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28442 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28443 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28444
28445
28446 calibrator = null;
28447 try {
28448 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28449 new Matrix(1, 1));
28450 fail("IllegalArgumentException expected but not thrown");
28451 } catch (final IllegalArgumentException ignore) {
28452 }
28453 try {
28454 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28455 new Matrix(1, 3));
28456 fail("IllegalArgumentException expected but not thrown");
28457 } catch (final IllegalArgumentException ignore) {
28458 }
28459 assertNull(calibrator);
28460 }
28461
28462 @Test
28463 public void testConstructor220() throws WrongSizeException {
28464 final Matrix ba = generateBa();
28465 final double[] bias = ba.getBuffer();
28466 final double biasX = ba.getElementAtIndex(0);
28467 final double biasY = ba.getElementAtIndex(1);
28468 final double biasZ = ba.getElementAtIndex(2);
28469
28470 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28471 final double latitude = Math.toRadians(
28472 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28473 final double longitude = Math.toRadians(
28474 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28475 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28476 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28477 final NEDVelocity nedVelocity = new NEDVelocity();
28478 final ECEFPosition ecefPosition = new ECEFPosition();
28479 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28480 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28481 ecefPosition, ecefVelocity);
28482
28483 KnownPositionAccelerometerCalibrator calibrator =
28484 new KnownPositionAccelerometerCalibrator(nedPosition, ba,
28485 this);
28486
28487
28488 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28489 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28490 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28491 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28492 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28493 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28494 final Acceleration bx2 = new Acceleration(0.0,
28495 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28496 calibrator.getInitialBiasXAsAcceleration(bx2);
28497 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28498 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28499 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28500 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28501 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28502 final Acceleration by2 = new Acceleration(0.0,
28503 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28504 calibrator.getInitialBiasYAsAcceleration(by2);
28505 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28506 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28507 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28508 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28509 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28510 final Acceleration bz2 = new Acceleration(0.0,
28511 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28512 calibrator.getInitialBiasZAsAcceleration(bz2);
28513 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28514 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28515 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28516 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28517 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28518 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28519 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28520 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28521 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28522 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28523 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28524 final double[] bias1 = calibrator.getInitialBias();
28525 assertArrayEquals(bias1, bias, 0.0);
28526 final double[] bias2 = new double[3];
28527 calibrator.getInitialBias(bias2);
28528 assertArrayEquals(bias1, bias2, 0.0);
28529 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28530 assertEquals(b1, ba);
28531 final Matrix b2 = new Matrix(3, 1);
28532 calibrator.getInitialBiasAsMatrix(b2);
28533 assertEquals(b1, b2);
28534 final Matrix ma1 = calibrator.getInitialMa();
28535 assertEquals(ma1, new Matrix(3, 3));
28536 final Matrix ma2 = new Matrix(3, 3);
28537 calibrator.getInitialMa(ma2);
28538 assertEquals(ma1, ma2);
28539 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28540 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28541 final NEDPosition nedPosition1 = new NEDPosition();
28542 assertTrue(calibrator.getNedPosition(nedPosition1));
28543 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28544 assertNull(calibrator.getMeasurements());
28545 assertFalse(calibrator.isCommonAxisUsed());
28546 assertSame(calibrator.getListener(), this);
28547 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28548 assertFalse(calibrator.isReady());
28549 assertFalse(calibrator.isRunning());
28550 assertNull(calibrator.getEstimatedBiases());
28551 assertFalse(calibrator.getEstimatedBiases(null));
28552 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28553 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28554 assertNull(calibrator.getEstimatedBiasFx());
28555 assertNull(calibrator.getEstimatedBiasFy());
28556 assertNull(calibrator.getEstimatedBiasFz());
28557 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28558 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28559 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28560 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28561 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28562 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28563 assertNull(calibrator.getEstimatedMa());
28564 assertNull(calibrator.getEstimatedSx());
28565 assertNull(calibrator.getEstimatedSy());
28566 assertNull(calibrator.getEstimatedSz());
28567 assertNull(calibrator.getEstimatedMxy());
28568 assertNull(calibrator.getEstimatedMxz());
28569 assertNull(calibrator.getEstimatedMyx());
28570 assertNull(calibrator.getEstimatedMyz());
28571 assertNull(calibrator.getEstimatedMzx());
28572 assertNull(calibrator.getEstimatedMzy());
28573 assertNull(calibrator.getEstimatedCovariance());
28574 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28575 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28576 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28577 assertNotNull(calibrator.getGroundTruthGravityNorm());
28578 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28579 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28580 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28581 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28582 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28583 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28584 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28585
28586
28587 calibrator = null;
28588 try {
28589 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28590 new Matrix(1, 1), this);
28591 fail("IllegalArgumentException expected but not thrown");
28592 } catch (final IllegalArgumentException ignore) {
28593 }
28594 try {
28595 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28596 new Matrix(1, 3), this);
28597 fail("IllegalArgumentException expected but not thrown");
28598 } catch (final IllegalArgumentException ignore) {
28599 }
28600 assertNull(calibrator);
28601 }
28602
28603 @Test
28604 public void testConstructor221() throws WrongSizeException {
28605 final Collection<StandardDeviationBodyKinematics> measurements =
28606 Collections.emptyList();
28607
28608 final Matrix ba = generateBa();
28609 final double[] bias = ba.getBuffer();
28610 final double biasX = ba.getElementAtIndex(0);
28611 final double biasY = ba.getElementAtIndex(1);
28612 final double biasZ = ba.getElementAtIndex(2);
28613
28614 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28615 final double latitude = Math.toRadians(
28616 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28617 final double longitude = Math.toRadians(
28618 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28619 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28620 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28621 final NEDVelocity nedVelocity = new NEDVelocity();
28622 final ECEFPosition ecefPosition = new ECEFPosition();
28623 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28624 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28625 ecefPosition, ecefVelocity);
28626
28627 KnownPositionAccelerometerCalibrator calibrator =
28628 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28629 ba);
28630
28631
28632 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28633 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28634 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28635 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28636 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28637 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28638 final Acceleration bx2 = new Acceleration(0.0,
28639 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28640 calibrator.getInitialBiasXAsAcceleration(bx2);
28641 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28642 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28643 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28644 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28645 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28646 final Acceleration by2 = new Acceleration(0.0,
28647 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28648 calibrator.getInitialBiasYAsAcceleration(by2);
28649 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28650 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28651 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28652 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28653 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28654 final Acceleration bz2 = new Acceleration(0.0,
28655 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28656 calibrator.getInitialBiasZAsAcceleration(bz2);
28657 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28658 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28659 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28660 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28661 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28662 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28663 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28664 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28665 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28666 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28667 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28668 final double[] bias1 = calibrator.getInitialBias();
28669 assertArrayEquals(bias1, bias, 0.0);
28670 final double[] bias2 = new double[3];
28671 calibrator.getInitialBias(bias2);
28672 assertArrayEquals(bias1, bias2, 0.0);
28673 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28674 assertEquals(b1, ba);
28675 final Matrix b2 = new Matrix(3, 1);
28676 calibrator.getInitialBiasAsMatrix(b2);
28677 assertEquals(b1, b2);
28678 final Matrix ma1 = calibrator.getInitialMa();
28679 assertEquals(ma1, new Matrix(3, 3));
28680 final Matrix ma2 = new Matrix(3, 3);
28681 calibrator.getInitialMa(ma2);
28682 assertEquals(ma1, ma2);
28683 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28684 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28685 final NEDPosition nedPosition1 = new NEDPosition();
28686 assertTrue(calibrator.getNedPosition(nedPosition1));
28687 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28688 assertSame(calibrator.getMeasurements(), measurements);
28689 assertFalse(calibrator.isCommonAxisUsed());
28690 assertNull(calibrator.getListener());
28691 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28692 assertFalse(calibrator.isReady());
28693 assertFalse(calibrator.isRunning());
28694 assertNull(calibrator.getEstimatedBiases());
28695 assertFalse(calibrator.getEstimatedBiases(null));
28696 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28697 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28698 assertNull(calibrator.getEstimatedBiasFx());
28699 assertNull(calibrator.getEstimatedBiasFy());
28700 assertNull(calibrator.getEstimatedBiasFz());
28701 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28702 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28703 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28704 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28705 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28706 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28707 assertNull(calibrator.getEstimatedMa());
28708 assertNull(calibrator.getEstimatedSx());
28709 assertNull(calibrator.getEstimatedSy());
28710 assertNull(calibrator.getEstimatedSz());
28711 assertNull(calibrator.getEstimatedMxy());
28712 assertNull(calibrator.getEstimatedMxz());
28713 assertNull(calibrator.getEstimatedMyx());
28714 assertNull(calibrator.getEstimatedMyz());
28715 assertNull(calibrator.getEstimatedMzx());
28716 assertNull(calibrator.getEstimatedMzy());
28717 assertNull(calibrator.getEstimatedCovariance());
28718 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28719 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28720 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28721 assertNotNull(calibrator.getGroundTruthGravityNorm());
28722 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28723 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28724 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28725 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28726 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28727 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28728 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28729
28730
28731 calibrator = null;
28732 try {
28733 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28734 measurements, new Matrix(1, 1));
28735 fail("IllegalArgumentException expected but not thrown");
28736 } catch (final IllegalArgumentException ignore) {
28737 }
28738 try {
28739 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28740 measurements, new Matrix(1, 3));
28741 fail("IllegalArgumentException expected but not thrown");
28742 } catch (final IllegalArgumentException ignore) {
28743 }
28744 assertNull(calibrator);
28745 }
28746
28747 @Test
28748 public void testConstructor222() throws WrongSizeException {
28749 final Collection<StandardDeviationBodyKinematics> measurements =
28750 Collections.emptyList();
28751
28752 final Matrix ba = generateBa();
28753 final double[] bias = ba.getBuffer();
28754 final double biasX = ba.getElementAtIndex(0);
28755 final double biasY = ba.getElementAtIndex(1);
28756 final double biasZ = ba.getElementAtIndex(2);
28757
28758 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28759 final double latitude = Math.toRadians(
28760 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28761 final double longitude = Math.toRadians(
28762 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28763 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28764 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28765 final NEDVelocity nedVelocity = new NEDVelocity();
28766 final ECEFPosition ecefPosition = new ECEFPosition();
28767 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28768 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28769 ecefPosition, ecefVelocity);
28770
28771 KnownPositionAccelerometerCalibrator calibrator =
28772 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28773 ba, this);
28774
28775
28776 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28777 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28778 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28779 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28780 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28781 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28782 final Acceleration bx2 = new Acceleration(0.0,
28783 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28784 calibrator.getInitialBiasXAsAcceleration(bx2);
28785 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28786 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28787 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28788 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28789 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28790 final Acceleration by2 = new Acceleration(0.0,
28791 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28792 calibrator.getInitialBiasYAsAcceleration(by2);
28793 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28794 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28795 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28796 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28797 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28798 final Acceleration bz2 = new Acceleration(0.0,
28799 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28800 calibrator.getInitialBiasZAsAcceleration(bz2);
28801 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28802 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28803 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28804 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28805 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28806 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28807 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28808 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28809 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28810 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28811 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28812 final double[] bias1 = calibrator.getInitialBias();
28813 assertArrayEquals(bias1, bias, 0.0);
28814 final double[] bias2 = new double[3];
28815 calibrator.getInitialBias(bias2);
28816 assertArrayEquals(bias1, bias2, 0.0);
28817 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28818 assertEquals(b1, ba);
28819 final Matrix b2 = new Matrix(3, 1);
28820 calibrator.getInitialBiasAsMatrix(b2);
28821 assertEquals(b1, b2);
28822 final Matrix ma1 = calibrator.getInitialMa();
28823 assertEquals(ma1, new Matrix(3, 3));
28824 final Matrix ma2 = new Matrix(3, 3);
28825 calibrator.getInitialMa(ma2);
28826 assertEquals(ma1, ma2);
28827 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28828 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28829 final NEDPosition nedPosition1 = new NEDPosition();
28830 assertTrue(calibrator.getNedPosition(nedPosition1));
28831 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28832 assertSame(calibrator.getMeasurements(), measurements);
28833 assertFalse(calibrator.isCommonAxisUsed());
28834 assertSame(calibrator.getListener(), this);
28835 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28836 assertFalse(calibrator.isReady());
28837 assertFalse(calibrator.isRunning());
28838 assertNull(calibrator.getEstimatedBiases());
28839 assertFalse(calibrator.getEstimatedBiases(null));
28840 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28841 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28842 assertNull(calibrator.getEstimatedBiasFx());
28843 assertNull(calibrator.getEstimatedBiasFy());
28844 assertNull(calibrator.getEstimatedBiasFz());
28845 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28846 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28847 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28848 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28849 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28850 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28851 assertNull(calibrator.getEstimatedMa());
28852 assertNull(calibrator.getEstimatedSx());
28853 assertNull(calibrator.getEstimatedSy());
28854 assertNull(calibrator.getEstimatedSz());
28855 assertNull(calibrator.getEstimatedMxy());
28856 assertNull(calibrator.getEstimatedMxz());
28857 assertNull(calibrator.getEstimatedMyx());
28858 assertNull(calibrator.getEstimatedMyz());
28859 assertNull(calibrator.getEstimatedMzx());
28860 assertNull(calibrator.getEstimatedMzy());
28861 assertNull(calibrator.getEstimatedCovariance());
28862 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28863 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28864 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28865 assertNotNull(calibrator.getGroundTruthGravityNorm());
28866 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28867 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28868 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28869 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28870 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28871 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28872 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28873
28874
28875 calibrator = null;
28876 try {
28877 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28878 measurements, new Matrix(1, 1), this);
28879 fail("IllegalArgumentException expected but not thrown");
28880 } catch (final IllegalArgumentException ignore) {
28881 }
28882 try {
28883 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28884 measurements, new Matrix(1, 3), this);
28885 fail("IllegalArgumentException expected but not thrown");
28886 } catch (final IllegalArgumentException ignore) {
28887 }
28888 assertNull(calibrator);
28889 }
28890
28891 @Test
28892 public void testConstructor223() throws WrongSizeException {
28893 final Matrix ba = generateBa();
28894 final double[] bias = ba.getBuffer();
28895 final double biasX = ba.getElementAtIndex(0);
28896 final double biasY = ba.getElementAtIndex(1);
28897 final double biasZ = ba.getElementAtIndex(2);
28898
28899 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28900 final double latitude = Math.toRadians(
28901 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28902 final double longitude = Math.toRadians(
28903 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28904 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28905 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28906 final NEDVelocity nedVelocity = new NEDVelocity();
28907 final ECEFPosition ecefPosition = new ECEFPosition();
28908 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28909 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28910 ecefPosition, ecefVelocity);
28911
28912 KnownPositionAccelerometerCalibrator calibrator =
28913 new KnownPositionAccelerometerCalibrator(nedPosition,
28914 true, ba);
28915
28916
28917 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28918 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28919 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28920 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28921 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28922 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28923 final Acceleration bx2 = new Acceleration(0.0,
28924 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28925 calibrator.getInitialBiasXAsAcceleration(bx2);
28926 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28927 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28928 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28929 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28930 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28931 final Acceleration by2 = new Acceleration(0.0,
28932 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28933 calibrator.getInitialBiasYAsAcceleration(by2);
28934 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28935 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28936 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28937 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28938 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28939 final Acceleration bz2 = new Acceleration(0.0,
28940 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28941 calibrator.getInitialBiasZAsAcceleration(bz2);
28942 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28943 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28944 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28945 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28946 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28947 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28948 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28949 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28950 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28951 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28952 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28953 final double[] bias1 = calibrator.getInitialBias();
28954 assertArrayEquals(bias1, bias, 0.0);
28955 final double[] bias2 = new double[3];
28956 calibrator.getInitialBias(bias2);
28957 assertArrayEquals(bias1, bias2, 0.0);
28958 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28959 assertEquals(b1, ba);
28960 final Matrix b2 = new Matrix(3, 1);
28961 calibrator.getInitialBiasAsMatrix(b2);
28962 assertEquals(b1, b2);
28963 final Matrix ma1 = calibrator.getInitialMa();
28964 assertEquals(ma1, new Matrix(3, 3));
28965 final Matrix ma2 = new Matrix(3, 3);
28966 calibrator.getInitialMa(ma2);
28967 assertEquals(ma1, ma2);
28968 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28969 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28970 final NEDPosition nedPosition1 = new NEDPosition();
28971 assertTrue(calibrator.getNedPosition(nedPosition1));
28972 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28973 assertNull(calibrator.getMeasurements());
28974 assertTrue(calibrator.isCommonAxisUsed());
28975 assertNull(calibrator.getListener());
28976 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28977 assertFalse(calibrator.isReady());
28978 assertFalse(calibrator.isRunning());
28979 assertNull(calibrator.getEstimatedBiases());
28980 assertFalse(calibrator.getEstimatedBiases(null));
28981 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28982 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28983 assertNull(calibrator.getEstimatedBiasFx());
28984 assertNull(calibrator.getEstimatedBiasFy());
28985 assertNull(calibrator.getEstimatedBiasFz());
28986 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28987 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28988 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28989 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28990 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28991 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28992 assertNull(calibrator.getEstimatedMa());
28993 assertNull(calibrator.getEstimatedSx());
28994 assertNull(calibrator.getEstimatedSy());
28995 assertNull(calibrator.getEstimatedSz());
28996 assertNull(calibrator.getEstimatedMxy());
28997 assertNull(calibrator.getEstimatedMxz());
28998 assertNull(calibrator.getEstimatedMyx());
28999 assertNull(calibrator.getEstimatedMyz());
29000 assertNull(calibrator.getEstimatedMzx());
29001 assertNull(calibrator.getEstimatedMzy());
29002 assertNull(calibrator.getEstimatedCovariance());
29003 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29004 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29005 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29006 assertNotNull(calibrator.getGroundTruthGravityNorm());
29007 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29008 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29009 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29010 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29011 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29012 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29013 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29014
29015
29016 calibrator = null;
29017 try {
29018 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29019 true, new Matrix(1, 1));
29020 fail("IllegalArgumentException expected but not thrown");
29021 } catch (final IllegalArgumentException ignore) {
29022 }
29023 try {
29024 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29025 true, new Matrix(1, 3));
29026 fail("IllegalArgumentException expected but not thrown");
29027 } catch (final IllegalArgumentException ignore) {
29028 }
29029 assertNull(calibrator);
29030 }
29031
29032 @Test
29033 public void testConstructor224() throws WrongSizeException {
29034 final Matrix ba = generateBa();
29035 final double[] bias = ba.getBuffer();
29036 final double biasX = ba.getElementAtIndex(0);
29037 final double biasY = ba.getElementAtIndex(1);
29038 final double biasZ = ba.getElementAtIndex(2);
29039
29040 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29041 final double latitude = Math.toRadians(
29042 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29043 final double longitude = Math.toRadians(
29044 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29045 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29046 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29047 final NEDVelocity nedVelocity = new NEDVelocity();
29048 final ECEFPosition ecefPosition = new ECEFPosition();
29049 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29050 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29051 ecefPosition, ecefVelocity);
29052
29053 KnownPositionAccelerometerCalibrator calibrator =
29054 new KnownPositionAccelerometerCalibrator(nedPosition,
29055 true, ba, this);
29056
29057
29058 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29059 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29060 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29061 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29062 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29063 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29064 final Acceleration bx2 = new Acceleration(0.0,
29065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29066 calibrator.getInitialBiasXAsAcceleration(bx2);
29067 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29068 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29069 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29070 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29071 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29072 final Acceleration by2 = new Acceleration(0.0,
29073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29074 calibrator.getInitialBiasYAsAcceleration(by2);
29075 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29076 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29077 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29078 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29079 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29080 final Acceleration bz2 = new Acceleration(0.0,
29081 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29082 calibrator.getInitialBiasZAsAcceleration(bz2);
29083 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29084 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29085 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29086 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29087 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29088 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29089 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29090 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29091 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29092 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29093 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29094 final double[] bias1 = calibrator.getInitialBias();
29095 assertArrayEquals(bias1, bias, 0.0);
29096 final double[] bias2 = new double[3];
29097 calibrator.getInitialBias(bias2);
29098 assertArrayEquals(bias1, bias2, 0.0);
29099 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29100 assertEquals(b1, ba);
29101 final Matrix b2 = new Matrix(3, 1);
29102 calibrator.getInitialBiasAsMatrix(b2);
29103 assertEquals(b1, b2);
29104 final Matrix ma1 = calibrator.getInitialMa();
29105 assertEquals(ma1, new Matrix(3, 3));
29106 final Matrix ma2 = new Matrix(3, 3);
29107 calibrator.getInitialMa(ma2);
29108 assertEquals(ma1, ma2);
29109 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29110 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29111 final NEDPosition nedPosition1 = new NEDPosition();
29112 assertTrue(calibrator.getNedPosition(nedPosition1));
29113 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29114 assertNull(calibrator.getMeasurements());
29115 assertTrue(calibrator.isCommonAxisUsed());
29116 assertSame(calibrator.getListener(), this);
29117 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29118 assertFalse(calibrator.isReady());
29119 assertFalse(calibrator.isRunning());
29120 assertNull(calibrator.getEstimatedBiases());
29121 assertFalse(calibrator.getEstimatedBiases(null));
29122 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29123 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29124 assertNull(calibrator.getEstimatedBiasFx());
29125 assertNull(calibrator.getEstimatedBiasFy());
29126 assertNull(calibrator.getEstimatedBiasFz());
29127 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29128 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29129 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29130 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29131 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29132 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29133 assertNull(calibrator.getEstimatedMa());
29134 assertNull(calibrator.getEstimatedSx());
29135 assertNull(calibrator.getEstimatedSy());
29136 assertNull(calibrator.getEstimatedSz());
29137 assertNull(calibrator.getEstimatedMxy());
29138 assertNull(calibrator.getEstimatedMxz());
29139 assertNull(calibrator.getEstimatedMyx());
29140 assertNull(calibrator.getEstimatedMyz());
29141 assertNull(calibrator.getEstimatedMzx());
29142 assertNull(calibrator.getEstimatedMzy());
29143 assertNull(calibrator.getEstimatedCovariance());
29144 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29145 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29146 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29147 assertNotNull(calibrator.getGroundTruthGravityNorm());
29148 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29149 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29150 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29151 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29152 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29153 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29154 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29155
29156
29157 calibrator = null;
29158 try {
29159 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29160 true, new Matrix(1, 1),
29161 this);
29162 fail("IllegalArgumentException expected but not thrown");
29163 } catch (final IllegalArgumentException ignore) {
29164 }
29165 try {
29166 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29167 true, new Matrix(1, 3),
29168 this);
29169 fail("IllegalArgumentException expected but not thrown");
29170 } catch (final IllegalArgumentException ignore) {
29171 }
29172 assertNull(calibrator);
29173 }
29174
29175 @Test
29176 public void testConstructor225() throws WrongSizeException {
29177 final Collection<StandardDeviationBodyKinematics> measurements =
29178 Collections.emptyList();
29179
29180 final Matrix ba = generateBa();
29181 final double[] bias = ba.getBuffer();
29182 final double biasX = ba.getElementAtIndex(0);
29183 final double biasY = ba.getElementAtIndex(1);
29184 final double biasZ = ba.getElementAtIndex(2);
29185
29186 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29187 final double latitude = Math.toRadians(
29188 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29189 final double longitude = Math.toRadians(
29190 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29191 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29192 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29193 final NEDVelocity nedVelocity = new NEDVelocity();
29194 final ECEFPosition ecefPosition = new ECEFPosition();
29195 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29196 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29197 ecefPosition, ecefVelocity);
29198
29199 KnownPositionAccelerometerCalibrator calibrator =
29200 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29201 true, ba);
29202
29203
29204 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29205 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29206 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29207 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29208 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29209 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29210 final Acceleration bx2 = new Acceleration(0.0,
29211 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29212 calibrator.getInitialBiasXAsAcceleration(bx2);
29213 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29214 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29215 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29216 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29217 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29218 final Acceleration by2 = new Acceleration(0.0,
29219 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29220 calibrator.getInitialBiasYAsAcceleration(by2);
29221 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29222 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29223 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29224 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29225 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29226 final Acceleration bz2 = new Acceleration(0.0,
29227 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29228 calibrator.getInitialBiasZAsAcceleration(bz2);
29229 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29230 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29231 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29232 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29233 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29234 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29235 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29236 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29237 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29238 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29239 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29240 final double[] bias1 = calibrator.getInitialBias();
29241 assertArrayEquals(bias1, bias, 0.0);
29242 final double[] bias2 = new double[3];
29243 calibrator.getInitialBias(bias2);
29244 assertArrayEquals(bias1, bias2, 0.0);
29245 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29246 assertEquals(b1, ba);
29247 final Matrix b2 = new Matrix(3, 1);
29248 calibrator.getInitialBiasAsMatrix(b2);
29249 assertEquals(b1, b2);
29250 final Matrix ma1 = calibrator.getInitialMa();
29251 assertEquals(ma1, new Matrix(3, 3));
29252 final Matrix ma2 = new Matrix(3, 3);
29253 calibrator.getInitialMa(ma2);
29254 assertEquals(ma1, ma2);
29255 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29256 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29257 final NEDPosition nedPosition1 = new NEDPosition();
29258 assertTrue(calibrator.getNedPosition(nedPosition1));
29259 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29260 assertSame(calibrator.getMeasurements(), measurements);
29261 assertTrue(calibrator.isCommonAxisUsed());
29262 assertNull(calibrator.getListener());
29263 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29264 assertFalse(calibrator.isReady());
29265 assertFalse(calibrator.isRunning());
29266 assertNull(calibrator.getEstimatedBiases());
29267 assertFalse(calibrator.getEstimatedBiases(null));
29268 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29269 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29270 assertNull(calibrator.getEstimatedBiasFx());
29271 assertNull(calibrator.getEstimatedBiasFy());
29272 assertNull(calibrator.getEstimatedBiasFz());
29273 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29274 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29275 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29276 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29277 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29278 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29279 assertNull(calibrator.getEstimatedMa());
29280 assertNull(calibrator.getEstimatedSx());
29281 assertNull(calibrator.getEstimatedSy());
29282 assertNull(calibrator.getEstimatedSz());
29283 assertNull(calibrator.getEstimatedMxy());
29284 assertNull(calibrator.getEstimatedMxz());
29285 assertNull(calibrator.getEstimatedMyx());
29286 assertNull(calibrator.getEstimatedMyz());
29287 assertNull(calibrator.getEstimatedMzx());
29288 assertNull(calibrator.getEstimatedMzy());
29289 assertNull(calibrator.getEstimatedCovariance());
29290 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29291 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29292 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29293 assertNotNull(calibrator.getGroundTruthGravityNorm());
29294 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29295 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29296 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29297 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29298 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29299 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29300 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29301
29302
29303 calibrator = null;
29304 try {
29305 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29306 measurements, true,
29307 new Matrix(1, 1));
29308 fail("IllegalArgumentException expected but not thrown");
29309 } catch (final IllegalArgumentException ignore) {
29310 }
29311 try {
29312 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29313 measurements, true,
29314 new Matrix(1, 3));
29315 fail("IllegalArgumentException expected but not thrown");
29316 } catch (final IllegalArgumentException ignore) {
29317 }
29318 assertNull(calibrator);
29319 }
29320
29321 @Test
29322 public void testConstructor226() throws WrongSizeException {
29323 final Collection<StandardDeviationBodyKinematics> measurements =
29324 Collections.emptyList();
29325
29326 final Matrix ba = generateBa();
29327 final double[] bias = ba.getBuffer();
29328 final double biasX = ba.getElementAtIndex(0);
29329 final double biasY = ba.getElementAtIndex(1);
29330 final double biasZ = ba.getElementAtIndex(2);
29331
29332 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29333 final double latitude = Math.toRadians(
29334 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29335 final double longitude = Math.toRadians(
29336 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29337 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29338 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29339 final NEDVelocity nedVelocity = new NEDVelocity();
29340 final ECEFPosition ecefPosition = new ECEFPosition();
29341 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29342 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29343 ecefPosition, ecefVelocity);
29344
29345 KnownPositionAccelerometerCalibrator calibrator =
29346 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29347 true, ba, this);
29348
29349
29350 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29351 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29352 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29353 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29354 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29355 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29356 final Acceleration bx2 = new Acceleration(0.0,
29357 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29358 calibrator.getInitialBiasXAsAcceleration(bx2);
29359 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29360 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29361 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29362 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29363 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29364 final Acceleration by2 = new Acceleration(0.0,
29365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29366 calibrator.getInitialBiasYAsAcceleration(by2);
29367 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29368 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29369 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29370 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29371 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29372 final Acceleration bz2 = new Acceleration(0.0,
29373 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29374 calibrator.getInitialBiasZAsAcceleration(bz2);
29375 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29376 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29377 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29378 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29379 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29380 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29381 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29382 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29383 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29384 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29385 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29386 final double[] bias1 = calibrator.getInitialBias();
29387 assertArrayEquals(bias1, bias, 0.0);
29388 final double[] bias2 = new double[3];
29389 calibrator.getInitialBias(bias2);
29390 assertArrayEquals(bias1, bias2, 0.0);
29391 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29392 assertEquals(b1, ba);
29393 final Matrix b2 = new Matrix(3, 1);
29394 calibrator.getInitialBiasAsMatrix(b2);
29395 assertEquals(b1, b2);
29396 final Matrix ma1 = calibrator.getInitialMa();
29397 assertEquals(ma1, new Matrix(3, 3));
29398 final Matrix ma2 = new Matrix(3, 3);
29399 calibrator.getInitialMa(ma2);
29400 assertEquals(ma1, ma2);
29401 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29402 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29403 final NEDPosition nedPosition1 = new NEDPosition();
29404 assertTrue(calibrator.getNedPosition(nedPosition1));
29405 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29406 assertSame(calibrator.getMeasurements(), measurements);
29407 assertTrue(calibrator.isCommonAxisUsed());
29408 assertSame(calibrator.getListener(), this);
29409 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29410 assertFalse(calibrator.isReady());
29411 assertFalse(calibrator.isRunning());
29412 assertNull(calibrator.getEstimatedBiases());
29413 assertFalse(calibrator.getEstimatedBiases(null));
29414 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29415 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29416 assertNull(calibrator.getEstimatedBiasFx());
29417 assertNull(calibrator.getEstimatedBiasFy());
29418 assertNull(calibrator.getEstimatedBiasFz());
29419 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29420 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29421 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29422 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29423 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29424 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29425 assertNull(calibrator.getEstimatedMa());
29426 assertNull(calibrator.getEstimatedSx());
29427 assertNull(calibrator.getEstimatedSy());
29428 assertNull(calibrator.getEstimatedSz());
29429 assertNull(calibrator.getEstimatedMxy());
29430 assertNull(calibrator.getEstimatedMxz());
29431 assertNull(calibrator.getEstimatedMyx());
29432 assertNull(calibrator.getEstimatedMyz());
29433 assertNull(calibrator.getEstimatedMzx());
29434 assertNull(calibrator.getEstimatedMzy());
29435 assertNull(calibrator.getEstimatedCovariance());
29436 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29437 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29438 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29439 assertNotNull(calibrator.getGroundTruthGravityNorm());
29440 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29441 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29442 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29443 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29444 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29445 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29446 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29447
29448
29449 calibrator = null;
29450 try {
29451 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29452 measurements, true,
29453 new Matrix(1, 1), this);
29454 fail("IllegalArgumentException expected but not thrown");
29455 } catch (final IllegalArgumentException ignore) {
29456 }
29457 try {
29458 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29459 measurements, true,
29460 new Matrix(1, 3), this);
29461 fail("IllegalArgumentException expected but not thrown");
29462 } catch (final IllegalArgumentException ignore) {
29463 }
29464 assertNull(calibrator);
29465 }
29466
29467 @Test
29468 public void testConstructor227() throws WrongSizeException {
29469 final Matrix ba = generateBa();
29470 final double[] bias = ba.getBuffer();
29471 final double biasX = ba.getElementAtIndex(0);
29472 final double biasY = ba.getElementAtIndex(1);
29473 final double biasZ = ba.getElementAtIndex(2);
29474
29475 final Matrix ma = generateMaCommonAxis();
29476 final double sx = ma.getElementAt(0, 0);
29477 final double sy = ma.getElementAt(1, 1);
29478 final double sz = ma.getElementAt(2, 2);
29479 final double mxy = ma.getElementAt(0, 1);
29480 final double mxz = ma.getElementAt(0, 2);
29481 final double myx = ma.getElementAt(1, 0);
29482 final double myz = ma.getElementAt(1, 2);
29483 final double mzx = ma.getElementAt(2, 0);
29484 final double mzy = ma.getElementAt(2, 1);
29485
29486 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29487 final double latitude = Math.toRadians(
29488 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29489 final double longitude = Math.toRadians(
29490 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29491 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29492 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29493 final NEDVelocity nedVelocity = new NEDVelocity();
29494 final ECEFPosition ecefPosition = new ECEFPosition();
29495 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29496 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29497 ecefPosition, ecefVelocity);
29498
29499 KnownPositionAccelerometerCalibrator calibrator =
29500 new KnownPositionAccelerometerCalibrator(nedPosition, ba, ma);
29501
29502
29503 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29504 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29505 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29506 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29507 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29508 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29509 final Acceleration bx2 = new Acceleration(0.0,
29510 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29511 calibrator.getInitialBiasXAsAcceleration(bx2);
29512 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29513 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29514 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29515 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29516 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29517 final Acceleration by2 = new Acceleration(0.0,
29518 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29519 calibrator.getInitialBiasYAsAcceleration(by2);
29520 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29521 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29522 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29523 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29524 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29525 final Acceleration bz2 = new Acceleration(0.0,
29526 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29527 calibrator.getInitialBiasZAsAcceleration(bz2);
29528 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29529 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29530 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29531 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29532 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29533 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29534 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29535 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29536 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29537 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29538 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29539 final double[] bias1 = calibrator.getInitialBias();
29540 assertArrayEquals(bias1, bias, 0.0);
29541 final double[] bias2 = new double[3];
29542 calibrator.getInitialBias(bias2);
29543 assertArrayEquals(bias1, bias2, 0.0);
29544 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29545 assertEquals(b1, ba);
29546 final Matrix b2 = new Matrix(3, 1);
29547 calibrator.getInitialBiasAsMatrix(b2);
29548 assertEquals(b1, b2);
29549 final Matrix ma1 = new Matrix(3, 3);
29550 ma1.setSubmatrix(0, 0,
29551 2, 2,
29552 new double[]{sx, myx, mzx,
29553 mxy, sy, mzy,
29554 mxz, myz, sz});
29555 assertEquals(calibrator.getInitialMa(), ma1);
29556 final Matrix ma2 = new Matrix(3, 3);
29557 calibrator.getInitialMa(ma2);
29558 assertEquals(ma1, ma2);
29559 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29560 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29561 final NEDPosition nedPosition1 = new NEDPosition();
29562 assertTrue(calibrator.getNedPosition(nedPosition1));
29563 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29564 assertNull(calibrator.getMeasurements());
29565 assertFalse(calibrator.isCommonAxisUsed());
29566 assertNull(calibrator.getListener());
29567 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29568 assertFalse(calibrator.isReady());
29569 assertFalse(calibrator.isRunning());
29570 assertNull(calibrator.getEstimatedBiases());
29571 assertFalse(calibrator.getEstimatedBiases(null));
29572 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29573 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29574 assertNull(calibrator.getEstimatedBiasFx());
29575 assertNull(calibrator.getEstimatedBiasFy());
29576 assertNull(calibrator.getEstimatedBiasFz());
29577 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29578 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29579 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29580 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29581 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29582 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29583 assertNull(calibrator.getEstimatedMa());
29584 assertNull(calibrator.getEstimatedSx());
29585 assertNull(calibrator.getEstimatedSy());
29586 assertNull(calibrator.getEstimatedSz());
29587 assertNull(calibrator.getEstimatedMxy());
29588 assertNull(calibrator.getEstimatedMxz());
29589 assertNull(calibrator.getEstimatedMyx());
29590 assertNull(calibrator.getEstimatedMyz());
29591 assertNull(calibrator.getEstimatedMzx());
29592 assertNull(calibrator.getEstimatedMzy());
29593 assertNull(calibrator.getEstimatedCovariance());
29594 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29595 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29596 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29597 assertNotNull(calibrator.getGroundTruthGravityNorm());
29598 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29599 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29600 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29601 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29602 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29603 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29604 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29605
29606
29607 calibrator = null;
29608 try {
29609 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29610 new Matrix(1, 1), ma);
29611 fail("IllegalArgumentException expected but not thrown");
29612 } catch (final IllegalArgumentException ignore) {
29613 }
29614 try {
29615 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29616 new Matrix(1, 3), ma);
29617 fail("IllegalArgumentException expected but not thrown");
29618 } catch (final IllegalArgumentException ignore) {
29619 }
29620 try {
29621 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29622 ba, new Matrix(1, 3));
29623 fail("IllegalArgumentException expected but not thrown");
29624 } catch (final IllegalArgumentException ignore) {
29625 }
29626 try {
29627 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29628 ba, new Matrix(3, 1));
29629 fail("IllegalArgumentException expected but not thrown");
29630 } catch (final IllegalArgumentException ignore) {
29631 }
29632 assertNull(calibrator);
29633 }
29634
29635 @Test
29636 public void testConstructor228() throws WrongSizeException {
29637 final Matrix ba = generateBa();
29638 final double[] bias = ba.getBuffer();
29639 final double biasX = ba.getElementAtIndex(0);
29640 final double biasY = ba.getElementAtIndex(1);
29641 final double biasZ = ba.getElementAtIndex(2);
29642
29643 final Matrix ma = generateMaCommonAxis();
29644 final double sx = ma.getElementAt(0, 0);
29645 final double sy = ma.getElementAt(1, 1);
29646 final double sz = ma.getElementAt(2, 2);
29647 final double mxy = ma.getElementAt(0, 1);
29648 final double mxz = ma.getElementAt(0, 2);
29649 final double myx = ma.getElementAt(1, 0);
29650 final double myz = ma.getElementAt(1, 2);
29651 final double mzx = ma.getElementAt(2, 0);
29652 final double mzy = ma.getElementAt(2, 1);
29653
29654 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29655 final double latitude = Math.toRadians(
29656 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29657 final double longitude = Math.toRadians(
29658 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29659 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29660 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29661 final NEDVelocity nedVelocity = new NEDVelocity();
29662 final ECEFPosition ecefPosition = new ECEFPosition();
29663 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29664 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29665 ecefPosition, ecefVelocity);
29666
29667 KnownPositionAccelerometerCalibrator calibrator =
29668 new KnownPositionAccelerometerCalibrator(nedPosition,
29669 ba, ma, this);
29670
29671
29672 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29673 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29674 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29675 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29676 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29677 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29678 final Acceleration bx2 = new Acceleration(0.0,
29679 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29680 calibrator.getInitialBiasXAsAcceleration(bx2);
29681 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29682 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29683 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29684 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29685 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29686 final Acceleration by2 = new Acceleration(0.0,
29687 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29688 calibrator.getInitialBiasYAsAcceleration(by2);
29689 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29690 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29691 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29692 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29693 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29694 final Acceleration bz2 = new Acceleration(0.0,
29695 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29696 calibrator.getInitialBiasZAsAcceleration(bz2);
29697 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29698 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29699 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29700 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29701 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29702 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29703 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29704 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29705 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29706 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29707 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29708 final double[] bias1 = calibrator.getInitialBias();
29709 assertArrayEquals(bias1, bias, 0.0);
29710 final double[] bias2 = new double[3];
29711 calibrator.getInitialBias(bias2);
29712 assertArrayEquals(bias1, bias2, 0.0);
29713 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29714 assertEquals(b1, ba);
29715 final Matrix b2 = new Matrix(3, 1);
29716 calibrator.getInitialBiasAsMatrix(b2);
29717 assertEquals(b1, b2);
29718 final Matrix ma1 = new Matrix(3, 3);
29719 ma1.setSubmatrix(0, 0,
29720 2, 2,
29721 new double[]{sx, myx, mzx,
29722 mxy, sy, mzy,
29723 mxz, myz, sz});
29724 assertEquals(calibrator.getInitialMa(), ma1);
29725 final Matrix ma2 = new Matrix(3, 3);
29726 calibrator.getInitialMa(ma2);
29727 assertEquals(ma1, ma2);
29728 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29729 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29730 final NEDPosition nedPosition1 = new NEDPosition();
29731 assertTrue(calibrator.getNedPosition(nedPosition1));
29732 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29733 assertNull(calibrator.getMeasurements());
29734 assertFalse(calibrator.isCommonAxisUsed());
29735 assertSame(calibrator.getListener(), this);
29736 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29737 assertFalse(calibrator.isReady());
29738 assertFalse(calibrator.isRunning());
29739 assertNull(calibrator.getEstimatedBiases());
29740 assertFalse(calibrator.getEstimatedBiases(null));
29741 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29742 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29743 assertNull(calibrator.getEstimatedBiasFx());
29744 assertNull(calibrator.getEstimatedBiasFy());
29745 assertNull(calibrator.getEstimatedBiasFz());
29746 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29747 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29748 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29749 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29750 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29751 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29752 assertNull(calibrator.getEstimatedMa());
29753 assertNull(calibrator.getEstimatedSx());
29754 assertNull(calibrator.getEstimatedSy());
29755 assertNull(calibrator.getEstimatedSz());
29756 assertNull(calibrator.getEstimatedMxy());
29757 assertNull(calibrator.getEstimatedMxz());
29758 assertNull(calibrator.getEstimatedMyx());
29759 assertNull(calibrator.getEstimatedMyz());
29760 assertNull(calibrator.getEstimatedMzx());
29761 assertNull(calibrator.getEstimatedMzy());
29762 assertNull(calibrator.getEstimatedCovariance());
29763 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29764 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29765 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29766 assertNotNull(calibrator.getGroundTruthGravityNorm());
29767 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29768 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29769 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29770 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29771 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29772 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29773 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29774
29775
29776 calibrator = null;
29777 try {
29778 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29779 new Matrix(1, 1), ma, this);
29780 fail("IllegalArgumentException expected but not thrown");
29781 } catch (final IllegalArgumentException ignore) {
29782 }
29783 try {
29784 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29785 new Matrix(1, 3), ma, this);
29786 fail("IllegalArgumentException expected but not thrown");
29787 } catch (final IllegalArgumentException ignore) {
29788 }
29789 try {
29790 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29791 ba, new Matrix(1, 3), this);
29792 fail("IllegalArgumentException expected but not thrown");
29793 } catch (final IllegalArgumentException ignore) {
29794 }
29795 try {
29796 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29797 ba, new Matrix(3, 1), this);
29798 fail("IllegalArgumentException expected but not thrown");
29799 } catch (final IllegalArgumentException ignore) {
29800 }
29801 assertNull(calibrator);
29802 }
29803
29804 @Test
29805 public void testConstructor229() throws WrongSizeException {
29806 final Collection<StandardDeviationBodyKinematics> measurements =
29807 Collections.emptyList();
29808
29809 final Matrix ba = generateBa();
29810 final double[] bias = ba.getBuffer();
29811 final double biasX = ba.getElementAtIndex(0);
29812 final double biasY = ba.getElementAtIndex(1);
29813 final double biasZ = ba.getElementAtIndex(2);
29814
29815 final Matrix ma = generateMaCommonAxis();
29816 final double sx = ma.getElementAt(0, 0);
29817 final double sy = ma.getElementAt(1, 1);
29818 final double sz = ma.getElementAt(2, 2);
29819 final double mxy = ma.getElementAt(0, 1);
29820 final double mxz = ma.getElementAt(0, 2);
29821 final double myx = ma.getElementAt(1, 0);
29822 final double myz = ma.getElementAt(1, 2);
29823 final double mzx = ma.getElementAt(2, 0);
29824 final double mzy = ma.getElementAt(2, 1);
29825
29826 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29827 final double latitude = Math.toRadians(
29828 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29829 final double longitude = Math.toRadians(
29830 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29831 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29832 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29833 final NEDVelocity nedVelocity = new NEDVelocity();
29834 final ECEFPosition ecefPosition = new ECEFPosition();
29835 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29836 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29837 ecefPosition, ecefVelocity);
29838
29839 KnownPositionAccelerometerCalibrator calibrator =
29840 new KnownPositionAccelerometerCalibrator(nedPosition,
29841 measurements, ba, ma);
29842
29843
29844 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29845 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29846 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29847 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29848 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29849 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29850 final Acceleration bx2 = new Acceleration(0.0,
29851 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29852 calibrator.getInitialBiasXAsAcceleration(bx2);
29853 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29854 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29855 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29856 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29857 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29858 final Acceleration by2 = new Acceleration(0.0,
29859 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29860 calibrator.getInitialBiasYAsAcceleration(by2);
29861 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29862 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29863 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29864 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29865 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29866 final Acceleration bz2 = new Acceleration(0.0,
29867 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29868 calibrator.getInitialBiasZAsAcceleration(bz2);
29869 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29870 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29871 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29872 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29873 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29874 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29875 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29876 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29877 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29878 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29879 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29880 final double[] bias1 = calibrator.getInitialBias();
29881 assertArrayEquals(bias1, bias, 0.0);
29882 final double[] bias2 = new double[3];
29883 calibrator.getInitialBias(bias2);
29884 assertArrayEquals(bias1, bias2, 0.0);
29885 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29886 assertEquals(b1, ba);
29887 final Matrix b2 = new Matrix(3, 1);
29888 calibrator.getInitialBiasAsMatrix(b2);
29889 assertEquals(b1, b2);
29890 final Matrix ma1 = new Matrix(3, 3);
29891 ma1.setSubmatrix(0, 0,
29892 2, 2,
29893 new double[]{sx, myx, mzx,
29894 mxy, sy, mzy,
29895 mxz, myz, sz});
29896 assertEquals(calibrator.getInitialMa(), ma1);
29897 final Matrix ma2 = new Matrix(3, 3);
29898 calibrator.getInitialMa(ma2);
29899 assertEquals(ma1, ma2);
29900 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29901 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29902 final NEDPosition nedPosition1 = new NEDPosition();
29903 assertTrue(calibrator.getNedPosition(nedPosition1));
29904 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29905 assertSame(calibrator.getMeasurements(), measurements);
29906 assertFalse(calibrator.isCommonAxisUsed());
29907 assertNull(calibrator.getListener());
29908 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29909 assertFalse(calibrator.isReady());
29910 assertFalse(calibrator.isRunning());
29911 assertNull(calibrator.getEstimatedBiases());
29912 assertFalse(calibrator.getEstimatedBiases(null));
29913 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29914 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29915 assertNull(calibrator.getEstimatedBiasFx());
29916 assertNull(calibrator.getEstimatedBiasFy());
29917 assertNull(calibrator.getEstimatedBiasFz());
29918 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29919 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29920 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29921 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29922 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29923 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29924 assertNull(calibrator.getEstimatedMa());
29925 assertNull(calibrator.getEstimatedSx());
29926 assertNull(calibrator.getEstimatedSy());
29927 assertNull(calibrator.getEstimatedSz());
29928 assertNull(calibrator.getEstimatedMxy());
29929 assertNull(calibrator.getEstimatedMxz());
29930 assertNull(calibrator.getEstimatedMyx());
29931 assertNull(calibrator.getEstimatedMyz());
29932 assertNull(calibrator.getEstimatedMzx());
29933 assertNull(calibrator.getEstimatedMzy());
29934 assertNull(calibrator.getEstimatedCovariance());
29935 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29936 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29937 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29938 assertNotNull(calibrator.getGroundTruthGravityNorm());
29939 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29940 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29941 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29942 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29943 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29944 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29945 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29946
29947
29948 calibrator = null;
29949 try {
29950 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29951 measurements, new Matrix(1, 1), ma);
29952 fail("IllegalArgumentException expected but not thrown");
29953 } catch (final IllegalArgumentException ignore) {
29954 }
29955 try {
29956 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29957 measurements, new Matrix(1, 3), ma);
29958 fail("IllegalArgumentException expected but not thrown");
29959 } catch (final IllegalArgumentException ignore) {
29960 }
29961 try {
29962 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29963 measurements, ba, new Matrix(1, 3));
29964 fail("IllegalArgumentException expected but not thrown");
29965 } catch (final IllegalArgumentException ignore) {
29966 }
29967 try {
29968 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29969 measurements, ba, new Matrix(3, 1));
29970 fail("IllegalArgumentException expected but not thrown");
29971 } catch (final IllegalArgumentException ignore) {
29972 }
29973 assertNull(calibrator);
29974 }
29975
29976 @Test
29977 public void testConstructor230() throws WrongSizeException {
29978 final Collection<StandardDeviationBodyKinematics> measurements =
29979 Collections.emptyList();
29980
29981 final Matrix ba = generateBa();
29982 final double[] bias = ba.getBuffer();
29983 final double biasX = ba.getElementAtIndex(0);
29984 final double biasY = ba.getElementAtIndex(1);
29985 final double biasZ = ba.getElementAtIndex(2);
29986
29987 final Matrix ma = generateMaCommonAxis();
29988 final double sx = ma.getElementAt(0, 0);
29989 final double sy = ma.getElementAt(1, 1);
29990 final double sz = ma.getElementAt(2, 2);
29991 final double mxy = ma.getElementAt(0, 1);
29992 final double mxz = ma.getElementAt(0, 2);
29993 final double myx = ma.getElementAt(1, 0);
29994 final double myz = ma.getElementAt(1, 2);
29995 final double mzx = ma.getElementAt(2, 0);
29996 final double mzy = ma.getElementAt(2, 1);
29997
29998 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29999 final double latitude = Math.toRadians(
30000 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30001 final double longitude = Math.toRadians(
30002 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30003 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30004 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30005 final NEDVelocity nedVelocity = new NEDVelocity();
30006 final ECEFPosition ecefPosition = new ECEFPosition();
30007 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30008 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30009 ecefPosition, ecefVelocity);
30010
30011 KnownPositionAccelerometerCalibrator calibrator =
30012 new KnownPositionAccelerometerCalibrator(nedPosition,
30013 measurements, ba, ma, this);
30014
30015
30016 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30017 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30018 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30019 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30020 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30021 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30022 final Acceleration bx2 = new Acceleration(0.0,
30023 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30024 calibrator.getInitialBiasXAsAcceleration(bx2);
30025 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30026 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30027 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30028 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30029 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30030 final Acceleration by2 = new Acceleration(0.0,
30031 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30032 calibrator.getInitialBiasYAsAcceleration(by2);
30033 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30034 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30035 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30036 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30037 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30038 final Acceleration bz2 = new Acceleration(0.0,
30039 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30040 calibrator.getInitialBiasZAsAcceleration(bz2);
30041 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30042 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30043 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30044 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30045 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30046 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30047 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30048 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30049 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30050 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30051 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30052 final double[] bias1 = calibrator.getInitialBias();
30053 assertArrayEquals(bias1, bias, 0.0);
30054 final double[] bias2 = new double[3];
30055 calibrator.getInitialBias(bias2);
30056 assertArrayEquals(bias1, bias2, 0.0);
30057 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30058 assertEquals(b1, ba);
30059 final Matrix b2 = new Matrix(3, 1);
30060 calibrator.getInitialBiasAsMatrix(b2);
30061 assertEquals(b1, b2);
30062 final Matrix ma1 = new Matrix(3, 3);
30063 ma1.setSubmatrix(0, 0,
30064 2, 2,
30065 new double[]{sx, myx, mzx,
30066 mxy, sy, mzy,
30067 mxz, myz, sz});
30068 assertEquals(calibrator.getInitialMa(), ma1);
30069 final Matrix ma2 = new Matrix(3, 3);
30070 calibrator.getInitialMa(ma2);
30071 assertEquals(ma1, ma2);
30072 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30073 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30074 final NEDPosition nedPosition1 = new NEDPosition();
30075 assertTrue(calibrator.getNedPosition(nedPosition1));
30076 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30077 assertSame(calibrator.getMeasurements(), measurements);
30078 assertFalse(calibrator.isCommonAxisUsed());
30079 assertSame(calibrator.getListener(), this);
30080 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30081 assertFalse(calibrator.isReady());
30082 assertFalse(calibrator.isRunning());
30083 assertNull(calibrator.getEstimatedBiases());
30084 assertFalse(calibrator.getEstimatedBiases(null));
30085 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30086 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30087 assertNull(calibrator.getEstimatedBiasFx());
30088 assertNull(calibrator.getEstimatedBiasFy());
30089 assertNull(calibrator.getEstimatedBiasFz());
30090 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30091 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30092 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30093 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30094 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30095 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30096 assertNull(calibrator.getEstimatedMa());
30097 assertNull(calibrator.getEstimatedSx());
30098 assertNull(calibrator.getEstimatedSy());
30099 assertNull(calibrator.getEstimatedSz());
30100 assertNull(calibrator.getEstimatedMxy());
30101 assertNull(calibrator.getEstimatedMxz());
30102 assertNull(calibrator.getEstimatedMyx());
30103 assertNull(calibrator.getEstimatedMyz());
30104 assertNull(calibrator.getEstimatedMzx());
30105 assertNull(calibrator.getEstimatedMzy());
30106 assertNull(calibrator.getEstimatedCovariance());
30107 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30108 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30109 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30110 assertNotNull(calibrator.getGroundTruthGravityNorm());
30111 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30112 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30113 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30114 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30115 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30116 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30117 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30118
30119
30120 calibrator = null;
30121 try {
30122 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30123 measurements, new Matrix(1, 1), ma, this);
30124 fail("IllegalArgumentException expected but not thrown");
30125 } catch (final IllegalArgumentException ignore) {
30126 }
30127 try {
30128 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30129 measurements, new Matrix(1, 3), ma, this);
30130 fail("IllegalArgumentException expected but not thrown");
30131 } catch (final IllegalArgumentException ignore) {
30132 }
30133 try {
30134 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30135 measurements, ba, new Matrix(1, 3), this);
30136 fail("IllegalArgumentException expected but not thrown");
30137 } catch (final IllegalArgumentException ignore) {
30138 }
30139 try {
30140 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30141 measurements, ba, new Matrix(3, 1), this);
30142 fail("IllegalArgumentException expected but not thrown");
30143 } catch (final IllegalArgumentException ignore) {
30144 }
30145 assertNull(calibrator);
30146 }
30147
30148 @Test
30149 public void testConstructor231() throws WrongSizeException {
30150 final Matrix ba = generateBa();
30151 final double[] bias = ba.getBuffer();
30152 final double biasX = ba.getElementAtIndex(0);
30153 final double biasY = ba.getElementAtIndex(1);
30154 final double biasZ = ba.getElementAtIndex(2);
30155
30156 final Matrix ma = generateMaCommonAxis();
30157 final double sx = ma.getElementAt(0, 0);
30158 final double sy = ma.getElementAt(1, 1);
30159 final double sz = ma.getElementAt(2, 2);
30160 final double mxy = ma.getElementAt(0, 1);
30161 final double mxz = ma.getElementAt(0, 2);
30162 final double myx = ma.getElementAt(1, 0);
30163 final double myz = ma.getElementAt(1, 2);
30164 final double mzx = ma.getElementAt(2, 0);
30165 final double mzy = ma.getElementAt(2, 1);
30166
30167 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30168 final double latitude = Math.toRadians(
30169 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30170 final double longitude = Math.toRadians(
30171 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30172 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30173 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30174 final NEDVelocity nedVelocity = new NEDVelocity();
30175 final ECEFPosition ecefPosition = new ECEFPosition();
30176 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30177 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30178 ecefPosition, ecefVelocity);
30179
30180 KnownPositionAccelerometerCalibrator calibrator =
30181 new KnownPositionAccelerometerCalibrator(nedPosition,
30182 true, ba, ma);
30183
30184
30185 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30186 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30187 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30188 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30189 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30190 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30191 final Acceleration bx2 = new Acceleration(0.0,
30192 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30193 calibrator.getInitialBiasXAsAcceleration(bx2);
30194 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30195 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30196 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30197 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30198 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30199 final Acceleration by2 = new Acceleration(0.0,
30200 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30201 calibrator.getInitialBiasYAsAcceleration(by2);
30202 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30203 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30204 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30205 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30206 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30207 final Acceleration bz2 = new Acceleration(0.0,
30208 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30209 calibrator.getInitialBiasZAsAcceleration(bz2);
30210 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30211 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30212 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30213 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30214 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30215 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30216 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30217 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30218 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30219 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30220 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30221 final double[] bias1 = calibrator.getInitialBias();
30222 assertArrayEquals(bias1, bias, 0.0);
30223 final double[] bias2 = new double[3];
30224 calibrator.getInitialBias(bias2);
30225 assertArrayEquals(bias1, bias2, 0.0);
30226 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30227 assertEquals(b1, ba);
30228 final Matrix b2 = new Matrix(3, 1);
30229 calibrator.getInitialBiasAsMatrix(b2);
30230 assertEquals(b1, b2);
30231 final Matrix ma1 = new Matrix(3, 3);
30232 ma1.setSubmatrix(0, 0,
30233 2, 2,
30234 new double[]{sx, myx, mzx,
30235 mxy, sy, mzy,
30236 mxz, myz, sz});
30237 assertEquals(calibrator.getInitialMa(), ma1);
30238 final Matrix ma2 = new Matrix(3, 3);
30239 calibrator.getInitialMa(ma2);
30240 assertEquals(ma1, ma2);
30241 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30242 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30243 final NEDPosition nedPosition1 = new NEDPosition();
30244 assertTrue(calibrator.getNedPosition(nedPosition1));
30245 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30246 assertNull(calibrator.getMeasurements());
30247 assertTrue(calibrator.isCommonAxisUsed());
30248 assertNull(calibrator.getListener());
30249 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30250 assertFalse(calibrator.isReady());
30251 assertFalse(calibrator.isRunning());
30252 assertNull(calibrator.getEstimatedBiases());
30253 assertFalse(calibrator.getEstimatedBiases(null));
30254 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30255 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30256 assertNull(calibrator.getEstimatedBiasFx());
30257 assertNull(calibrator.getEstimatedBiasFy());
30258 assertNull(calibrator.getEstimatedBiasFz());
30259 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30260 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30261 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30262 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30263 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30264 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30265 assertNull(calibrator.getEstimatedMa());
30266 assertNull(calibrator.getEstimatedSx());
30267 assertNull(calibrator.getEstimatedSy());
30268 assertNull(calibrator.getEstimatedSz());
30269 assertNull(calibrator.getEstimatedMxy());
30270 assertNull(calibrator.getEstimatedMxz());
30271 assertNull(calibrator.getEstimatedMyx());
30272 assertNull(calibrator.getEstimatedMyz());
30273 assertNull(calibrator.getEstimatedMzx());
30274 assertNull(calibrator.getEstimatedMzy());
30275 assertNull(calibrator.getEstimatedCovariance());
30276 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30277 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30278 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30279 assertNotNull(calibrator.getGroundTruthGravityNorm());
30280 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30281 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30282 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30283 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30284 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30285 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30286 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30287
30288
30289 calibrator = null;
30290 try {
30291 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30292 true, new Matrix(1, 1), ma);
30293 fail("IllegalArgumentException expected but not thrown");
30294 } catch (final IllegalArgumentException ignore) {
30295 }
30296 try {
30297 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30298 true, new Matrix(1, 3), ma);
30299 fail("IllegalArgumentException expected but not thrown");
30300 } catch (final IllegalArgumentException ignore) {
30301 }
30302 try {
30303 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30304 true, ba, new Matrix(1, 3));
30305 fail("IllegalArgumentException expected but not thrown");
30306 } catch (final IllegalArgumentException ignore) {
30307 }
30308 try {
30309 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30310 true, ba, new Matrix(3, 1));
30311 fail("IllegalArgumentException expected but not thrown");
30312 } catch (final IllegalArgumentException ignore) {
30313 }
30314 assertNull(calibrator);
30315 }
30316
30317 @Test
30318 public void testConstructor232() throws WrongSizeException {
30319 final Matrix ba = generateBa();
30320 final double[] bias = ba.getBuffer();
30321 final double biasX = ba.getElementAtIndex(0);
30322 final double biasY = ba.getElementAtIndex(1);
30323 final double biasZ = ba.getElementAtIndex(2);
30324
30325 final Matrix ma = generateMaCommonAxis();
30326 final double sx = ma.getElementAt(0, 0);
30327 final double sy = ma.getElementAt(1, 1);
30328 final double sz = ma.getElementAt(2, 2);
30329 final double mxy = ma.getElementAt(0, 1);
30330 final double mxz = ma.getElementAt(0, 2);
30331 final double myx = ma.getElementAt(1, 0);
30332 final double myz = ma.getElementAt(1, 2);
30333 final double mzx = ma.getElementAt(2, 0);
30334 final double mzy = ma.getElementAt(2, 1);
30335
30336 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30337 final double latitude = Math.toRadians(
30338 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30339 final double longitude = Math.toRadians(
30340 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30341 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30342 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30343 final NEDVelocity nedVelocity = new NEDVelocity();
30344 final ECEFPosition ecefPosition = new ECEFPosition();
30345 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30346 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30347 ecefPosition, ecefVelocity);
30348
30349 KnownPositionAccelerometerCalibrator calibrator =
30350 new KnownPositionAccelerometerCalibrator(nedPosition,
30351 true, ba, ma, this);
30352
30353
30354 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30355 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30356 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30357 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30358 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30359 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30360 final Acceleration bx2 = new Acceleration(0.0,
30361 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30362 calibrator.getInitialBiasXAsAcceleration(bx2);
30363 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30364 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30365 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30366 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30367 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30368 final Acceleration by2 = new Acceleration(0.0,
30369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30370 calibrator.getInitialBiasYAsAcceleration(by2);
30371 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30372 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30373 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30374 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30375 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30376 final Acceleration bz2 = new Acceleration(0.0,
30377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30378 calibrator.getInitialBiasZAsAcceleration(bz2);
30379 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30380 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30381 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30382 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30383 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30384 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30385 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30386 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30387 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30388 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30389 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30390 final double[] bias1 = calibrator.getInitialBias();
30391 assertArrayEquals(bias1, bias, 0.0);
30392 final double[] bias2 = new double[3];
30393 calibrator.getInitialBias(bias2);
30394 assertArrayEquals(bias1, bias2, 0.0);
30395 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30396 assertEquals(b1, ba);
30397 final Matrix b2 = new Matrix(3, 1);
30398 calibrator.getInitialBiasAsMatrix(b2);
30399 assertEquals(b1, b2);
30400 final Matrix ma1 = new Matrix(3, 3);
30401 ma1.setSubmatrix(0, 0,
30402 2, 2,
30403 new double[]{sx, myx, mzx,
30404 mxy, sy, mzy,
30405 mxz, myz, sz});
30406 assertEquals(calibrator.getInitialMa(), ma1);
30407 final Matrix ma2 = new Matrix(3, 3);
30408 calibrator.getInitialMa(ma2);
30409 assertEquals(ma1, ma2);
30410 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30411 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30412 final NEDPosition nedPosition1 = new NEDPosition();
30413 assertTrue(calibrator.getNedPosition(nedPosition1));
30414 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30415 assertNull(calibrator.getMeasurements());
30416 assertTrue(calibrator.isCommonAxisUsed());
30417 assertSame(calibrator.getListener(), this);
30418 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30419 assertFalse(calibrator.isReady());
30420 assertFalse(calibrator.isRunning());
30421 assertNull(calibrator.getEstimatedBiases());
30422 assertFalse(calibrator.getEstimatedBiases(null));
30423 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30424 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30425 assertNull(calibrator.getEstimatedBiasFx());
30426 assertNull(calibrator.getEstimatedBiasFy());
30427 assertNull(calibrator.getEstimatedBiasFz());
30428 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30429 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30430 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30431 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30432 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30433 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30434 assertNull(calibrator.getEstimatedMa());
30435 assertNull(calibrator.getEstimatedSx());
30436 assertNull(calibrator.getEstimatedSy());
30437 assertNull(calibrator.getEstimatedSz());
30438 assertNull(calibrator.getEstimatedMxy());
30439 assertNull(calibrator.getEstimatedMxz());
30440 assertNull(calibrator.getEstimatedMyx());
30441 assertNull(calibrator.getEstimatedMyz());
30442 assertNull(calibrator.getEstimatedMzx());
30443 assertNull(calibrator.getEstimatedMzy());
30444 assertNull(calibrator.getEstimatedCovariance());
30445 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30446 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30447 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30448 assertNotNull(calibrator.getGroundTruthGravityNorm());
30449 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30450 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30451 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30452 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30453 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30454 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30455 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30456
30457
30458 calibrator = null;
30459 try {
30460 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30461 true, new Matrix(1, 1), ma, this);
30462 fail("IllegalArgumentException expected but not thrown");
30463 } catch (final IllegalArgumentException ignore) {
30464 }
30465 try {
30466 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30467 true, new Matrix(1, 3), ma, this);
30468 fail("IllegalArgumentException expected but not thrown");
30469 } catch (final IllegalArgumentException ignore) {
30470 }
30471 try {
30472 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30473 true, ba, new Matrix(1, 3), this);
30474 fail("IllegalArgumentException expected but not thrown");
30475 } catch (final IllegalArgumentException ignore) {
30476 }
30477 try {
30478 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30479 true, ba, new Matrix(3, 1), this);
30480 fail("IllegalArgumentException expected but not thrown");
30481 } catch (final IllegalArgumentException ignore) {
30482 }
30483 assertNull(calibrator);
30484 }
30485
30486 @Test
30487 public void testConstructor233() throws WrongSizeException {
30488 final Collection<StandardDeviationBodyKinematics> measurements =
30489 Collections.emptyList();
30490
30491 final Matrix ba = generateBa();
30492 final double[] bias = ba.getBuffer();
30493 final double biasX = ba.getElementAtIndex(0);
30494 final double biasY = ba.getElementAtIndex(1);
30495 final double biasZ = ba.getElementAtIndex(2);
30496
30497 final Matrix ma = generateMaCommonAxis();
30498 final double sx = ma.getElementAt(0, 0);
30499 final double sy = ma.getElementAt(1, 1);
30500 final double sz = ma.getElementAt(2, 2);
30501 final double mxy = ma.getElementAt(0, 1);
30502 final double mxz = ma.getElementAt(0, 2);
30503 final double myx = ma.getElementAt(1, 0);
30504 final double myz = ma.getElementAt(1, 2);
30505 final double mzx = ma.getElementAt(2, 0);
30506 final double mzy = ma.getElementAt(2, 1);
30507
30508 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30509 final double latitude = Math.toRadians(
30510 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30511 final double longitude = Math.toRadians(
30512 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30513 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30514 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30515 final NEDVelocity nedVelocity = new NEDVelocity();
30516 final ECEFPosition ecefPosition = new ECEFPosition();
30517 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30518 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30519 ecefPosition, ecefVelocity);
30520
30521 KnownPositionAccelerometerCalibrator calibrator =
30522 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30523 true, ba, ma);
30524
30525
30526 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30527 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30528 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30529 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30530 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30531 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30532 final Acceleration bx2 = new Acceleration(0.0,
30533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30534 calibrator.getInitialBiasXAsAcceleration(bx2);
30535 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30536 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30537 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30538 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30539 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30540 final Acceleration by2 = new Acceleration(0.0,
30541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30542 calibrator.getInitialBiasYAsAcceleration(by2);
30543 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30544 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30545 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30546 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30547 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30548 final Acceleration bz2 = new Acceleration(0.0,
30549 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30550 calibrator.getInitialBiasZAsAcceleration(bz2);
30551 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30552 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30553 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30554 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30555 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30556 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30557 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30558 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30559 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30560 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30561 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30562 final double[] bias1 = calibrator.getInitialBias();
30563 assertArrayEquals(bias1, bias, 0.0);
30564 final double[] bias2 = new double[3];
30565 calibrator.getInitialBias(bias2);
30566 assertArrayEquals(bias1, bias2, 0.0);
30567 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30568 assertEquals(b1, ba);
30569 final Matrix b2 = new Matrix(3, 1);
30570 calibrator.getInitialBiasAsMatrix(b2);
30571 assertEquals(b1, b2);
30572 final Matrix ma1 = new Matrix(3, 3);
30573 ma1.setSubmatrix(0, 0,
30574 2, 2,
30575 new double[]{sx, myx, mzx,
30576 mxy, sy, mzy,
30577 mxz, myz, sz});
30578 assertEquals(calibrator.getInitialMa(), ma1);
30579 final Matrix ma2 = new Matrix(3, 3);
30580 calibrator.getInitialMa(ma2);
30581 assertEquals(ma1, ma2);
30582 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30583 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30584 final NEDPosition nedPosition1 = new NEDPosition();
30585 assertTrue(calibrator.getNedPosition(nedPosition1));
30586 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30587 assertSame(calibrator.getMeasurements(), measurements);
30588 assertTrue(calibrator.isCommonAxisUsed());
30589 assertNull(calibrator.getListener());
30590 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30591 assertFalse(calibrator.isReady());
30592 assertFalse(calibrator.isRunning());
30593 assertNull(calibrator.getEstimatedBiases());
30594 assertFalse(calibrator.getEstimatedBiases(null));
30595 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30596 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30597 assertNull(calibrator.getEstimatedBiasFx());
30598 assertNull(calibrator.getEstimatedBiasFy());
30599 assertNull(calibrator.getEstimatedBiasFz());
30600 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30601 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30602 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30603 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30604 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30605 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30606 assertNull(calibrator.getEstimatedMa());
30607 assertNull(calibrator.getEstimatedSx());
30608 assertNull(calibrator.getEstimatedSy());
30609 assertNull(calibrator.getEstimatedSz());
30610 assertNull(calibrator.getEstimatedMxy());
30611 assertNull(calibrator.getEstimatedMxz());
30612 assertNull(calibrator.getEstimatedMyx());
30613 assertNull(calibrator.getEstimatedMyz());
30614 assertNull(calibrator.getEstimatedMzx());
30615 assertNull(calibrator.getEstimatedMzy());
30616 assertNull(calibrator.getEstimatedCovariance());
30617 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30618 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30619 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30620 assertNotNull(calibrator.getGroundTruthGravityNorm());
30621 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30622 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30623 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30624 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30625 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30626 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30627 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30628
30629
30630 calibrator = null;
30631 try {
30632 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30633 measurements, true,
30634 new Matrix(1, 1), ma);
30635 fail("IllegalArgumentException expected but not thrown");
30636 } catch (final IllegalArgumentException ignore) {
30637 }
30638 try {
30639 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30640 measurements, true,
30641 new Matrix(1, 3), ma);
30642 fail("IllegalArgumentException expected but not thrown");
30643 } catch (final IllegalArgumentException ignore) {
30644 }
30645 try {
30646 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30647 measurements, true,
30648 ba, new Matrix(1, 3));
30649 fail("IllegalArgumentException expected but not thrown");
30650 } catch (final IllegalArgumentException ignore) {
30651 }
30652 try {
30653 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30654 measurements, true,
30655 ba, new Matrix(3, 1));
30656 fail("IllegalArgumentException expected but not thrown");
30657 } catch (final IllegalArgumentException ignore) {
30658 }
30659 assertNull(calibrator);
30660 }
30661
30662 @Test
30663 public void testConstructor234() throws WrongSizeException {
30664 final Collection<StandardDeviationBodyKinematics> measurements =
30665 Collections.emptyList();
30666
30667 final Matrix ba = generateBa();
30668 final double[] bias = ba.getBuffer();
30669 final double biasX = ba.getElementAtIndex(0);
30670 final double biasY = ba.getElementAtIndex(1);
30671 final double biasZ = ba.getElementAtIndex(2);
30672
30673 final Matrix ma = generateMaCommonAxis();
30674 final double sx = ma.getElementAt(0, 0);
30675 final double sy = ma.getElementAt(1, 1);
30676 final double sz = ma.getElementAt(2, 2);
30677 final double mxy = ma.getElementAt(0, 1);
30678 final double mxz = ma.getElementAt(0, 2);
30679 final double myx = ma.getElementAt(1, 0);
30680 final double myz = ma.getElementAt(1, 2);
30681 final double mzx = ma.getElementAt(2, 0);
30682 final double mzy = ma.getElementAt(2, 1);
30683
30684 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30685 final double latitude = Math.toRadians(
30686 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30687 final double longitude = Math.toRadians(
30688 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30689 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30690 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30691 final NEDVelocity nedVelocity = new NEDVelocity();
30692 final ECEFPosition ecefPosition = new ECEFPosition();
30693 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30694 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30695 ecefPosition, ecefVelocity);
30696
30697 KnownPositionAccelerometerCalibrator calibrator =
30698 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30699 true, ba, ma, this);
30700
30701
30702 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30703 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30704 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30705 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30706 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30707 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30708 final Acceleration bx2 = new Acceleration(0.0,
30709 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30710 calibrator.getInitialBiasXAsAcceleration(bx2);
30711 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30712 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30713 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30714 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30715 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30716 final Acceleration by2 = new Acceleration(0.0,
30717 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30718 calibrator.getInitialBiasYAsAcceleration(by2);
30719 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30720 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30721 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30722 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30723 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30724 final Acceleration bz2 = new Acceleration(0.0,
30725 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30726 calibrator.getInitialBiasZAsAcceleration(bz2);
30727 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30728 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30729 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30730 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30731 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30732 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30733 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30734 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30735 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30736 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30737 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30738 final double[] bias1 = calibrator.getInitialBias();
30739 assertArrayEquals(bias1, bias, 0.0);
30740 final double[] bias2 = new double[3];
30741 calibrator.getInitialBias(bias2);
30742 assertArrayEquals(bias1, bias2, 0.0);
30743 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30744 assertEquals(b1, ba);
30745 final Matrix b2 = new Matrix(3, 1);
30746 calibrator.getInitialBiasAsMatrix(b2);
30747 assertEquals(b1, b2);
30748 final Matrix ma1 = new Matrix(3, 3);
30749 ma1.setSubmatrix(0, 0,
30750 2, 2,
30751 new double[]{sx, myx, mzx,
30752 mxy, sy, mzy,
30753 mxz, myz, sz});
30754 assertEquals(calibrator.getInitialMa(), ma1);
30755 final Matrix ma2 = new Matrix(3, 3);
30756 calibrator.getInitialMa(ma2);
30757 assertEquals(ma1, ma2);
30758 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30759 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30760 final NEDPosition nedPosition1 = new NEDPosition();
30761 assertTrue(calibrator.getNedPosition(nedPosition1));
30762 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30763 assertSame(calibrator.getMeasurements(), measurements);
30764 assertTrue(calibrator.isCommonAxisUsed());
30765 assertSame(calibrator.getListener(), this);
30766 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30767 assertFalse(calibrator.isReady());
30768 assertFalse(calibrator.isRunning());
30769 assertNull(calibrator.getEstimatedBiases());
30770 assertFalse(calibrator.getEstimatedBiases(null));
30771 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30772 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30773 assertNull(calibrator.getEstimatedBiasFx());
30774 assertNull(calibrator.getEstimatedBiasFy());
30775 assertNull(calibrator.getEstimatedBiasFz());
30776 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30777 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30778 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30779 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30780 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30781 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30782 assertNull(calibrator.getEstimatedMa());
30783 assertNull(calibrator.getEstimatedSx());
30784 assertNull(calibrator.getEstimatedSy());
30785 assertNull(calibrator.getEstimatedSz());
30786 assertNull(calibrator.getEstimatedMxy());
30787 assertNull(calibrator.getEstimatedMxz());
30788 assertNull(calibrator.getEstimatedMyx());
30789 assertNull(calibrator.getEstimatedMyz());
30790 assertNull(calibrator.getEstimatedMzx());
30791 assertNull(calibrator.getEstimatedMzy());
30792 assertNull(calibrator.getEstimatedCovariance());
30793 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30794 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30795 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30796 assertNotNull(calibrator.getGroundTruthGravityNorm());
30797 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30798 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30799 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30800 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30801 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30802 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30803 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30804
30805
30806 calibrator = null;
30807 try {
30808 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30809 measurements, true,
30810 new Matrix(1, 1), ma, this);
30811 fail("IllegalArgumentException expected but not thrown");
30812 } catch (final IllegalArgumentException ignore) {
30813 }
30814 try {
30815 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30816 measurements, true,
30817 new Matrix(1, 3), ma, this);
30818 fail("IllegalArgumentException expected but not thrown");
30819 } catch (final IllegalArgumentException ignore) {
30820 }
30821 try {
30822 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30823 measurements, true,
30824 ba, new Matrix(1, 3), this);
30825 fail("IllegalArgumentException expected but not thrown");
30826 } catch (final IllegalArgumentException ignore) {
30827 }
30828 try {
30829 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30830 measurements, true,
30831 ba, new Matrix(3, 1), this);
30832 fail("IllegalArgumentException expected but not thrown");
30833 } catch (final IllegalArgumentException ignore) {
30834 }
30835 assertNull(calibrator);
30836 }
30837
30838 @Test
30839 public void testGetSetInitialBiasX() throws LockedException {
30840 final KnownPositionAccelerometerCalibrator calibrator =
30841 new KnownPositionAccelerometerCalibrator();
30842
30843 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30844
30845
30846 final Matrix ba = generateBa();
30847 final double biasX = ba.getElementAtIndex(0);
30848
30849 calibrator.setInitialBiasX(biasX);
30850
30851
30852 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30853 }
30854
30855 @Test
30856 public void testGetSetInitialBiasY() throws LockedException {
30857 final KnownPositionAccelerometerCalibrator calibrator =
30858 new KnownPositionAccelerometerCalibrator();
30859
30860 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30861
30862
30863 final Matrix ba = generateBa();
30864 final double biasY = ba.getElementAtIndex(1);
30865
30866 calibrator.setInitialBiasY(biasY);
30867
30868
30869 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30870 }
30871
30872 @Test
30873 public void testGetSetInitialBiasZ() throws LockedException {
30874 final KnownPositionAccelerometerCalibrator calibrator =
30875 new KnownPositionAccelerometerCalibrator();
30876
30877 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30878
30879
30880 final Matrix ba = generateBa();
30881 final double biasZ = ba.getElementAtIndex(2);
30882
30883 calibrator.setInitialBiasZ(biasZ);
30884
30885
30886 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30887 }
30888
30889 @Test
30890 public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
30891 final KnownPositionAccelerometerCalibrator calibrator =
30892 new KnownPositionAccelerometerCalibrator();
30893
30894
30895 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30896
30897 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
30898 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30899
30900
30901 final Matrix ba = generateBa();
30902 final double biasX = ba.getElementAtIndex(0);
30903
30904 final Acceleration bx2 = new Acceleration(biasX,
30905 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30906 calibrator.setInitialBiasX(bx2);
30907
30908
30909 final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
30910 final Acceleration bx4 = new Acceleration(biasX,
30911 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30912 calibrator.getInitialBiasXAsAcceleration(bx4);
30913
30914 assertEquals(bx3, bx4);
30915 }
30916
30917 @Test
30918 public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
30919 final KnownPositionAccelerometerCalibrator calibrator =
30920 new KnownPositionAccelerometerCalibrator();
30921
30922
30923 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30924
30925 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
30926 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30927
30928
30929 final Matrix ba = generateBa();
30930 final double biasY = ba.getElementAtIndex(1);
30931
30932 final Acceleration by2 = new Acceleration(biasY,
30933 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30934 calibrator.setInitialBiasY(by2);
30935
30936
30937 final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
30938 final Acceleration by4 = new Acceleration(biasY,
30939 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30940 calibrator.getInitialBiasYAsAcceleration(by4);
30941
30942 assertEquals(by3, by4);
30943 }
30944
30945 @Test
30946 public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
30947 final KnownPositionAccelerometerCalibrator calibrator =
30948 new KnownPositionAccelerometerCalibrator();
30949
30950
30951 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30952
30953 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
30954 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30955
30956
30957 final Matrix ba = generateBa();
30958 final double biasZ = ba.getElementAtIndex(2);
30959
30960 final Acceleration bz2 = new Acceleration(biasZ,
30961 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30962 calibrator.setInitialBiasZ(bz2);
30963
30964
30965 final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
30966 final Acceleration bz4 = new Acceleration(biasZ,
30967 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30968 calibrator.getInitialBiasZAsAcceleration(bz4);
30969
30970 assertEquals(bz3, bz4);
30971 }
30972
30973 @Test
30974 public void testSetInitialBias1() throws LockedException {
30975 final KnownPositionAccelerometerCalibrator calibrator =
30976 new KnownPositionAccelerometerCalibrator();
30977
30978
30979 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30980 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30981 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30982
30983
30984 final Matrix ba = generateBa();
30985 final double biasX = ba.getElementAtIndex(0);
30986 final double biasY = ba.getElementAtIndex(1);
30987 final double biasZ = ba.getElementAtIndex(2);
30988
30989 calibrator.setInitialBias(biasX, biasY, biasZ);
30990
30991
30992 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30993 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30994 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30995 }
30996
30997 @Test
30998 public void testSetInitialBias2() throws LockedException {
30999 final KnownPositionAccelerometerCalibrator calibrator =
31000 new KnownPositionAccelerometerCalibrator();
31001
31002
31003 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
31004 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
31005 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
31006
31007
31008 final Matrix ba = generateBa();
31009 final double biasX = ba.getElementAtIndex(0);
31010 final double biasY = ba.getElementAtIndex(1);
31011 final double biasZ = ba.getElementAtIndex(2);
31012
31013 final Acceleration bx = new Acceleration(biasX,
31014 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31015 final Acceleration by = new Acceleration(biasY,
31016 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31017 final Acceleration bz = new Acceleration(biasZ,
31018 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31019
31020 calibrator.setInitialBias(bx, by, bz);
31021
31022
31023 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31024 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31025 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31026 }
31027
31028 @Test
31029 public void testGetSetInitialSx() throws WrongSizeException, LockedException {
31030 final KnownPositionAccelerometerCalibrator calibrator =
31031 new KnownPositionAccelerometerCalibrator();
31032
31033 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31034
31035
31036 final Matrix ma = generateMaGeneral();
31037 final double sx = ma.getElementAt(0, 0);
31038
31039 calibrator.setInitialSx(sx);
31040
31041
31042 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31043 }
31044
31045 @Test
31046 public void testGetSetInitialSy() throws WrongSizeException, LockedException {
31047 final KnownPositionAccelerometerCalibrator calibrator =
31048 new KnownPositionAccelerometerCalibrator();
31049
31050 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31051
31052
31053 final Matrix ma = generateMaGeneral();
31054 final double sy = ma.getElementAt(1, 1);
31055
31056 calibrator.setInitialSy(sy);
31057
31058
31059 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31060 }
31061
31062 @Test
31063 public void testGetSetInitialSz() throws WrongSizeException, LockedException {
31064 final KnownPositionAccelerometerCalibrator calibrator =
31065 new KnownPositionAccelerometerCalibrator();
31066
31067 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31068
31069
31070 final Matrix ma = generateMaGeneral();
31071 final double sz = ma.getElementAt(2, 2);
31072
31073 calibrator.setInitialSz(sz);
31074
31075
31076 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31077 }
31078
31079 @Test
31080 public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
31081 final KnownPositionAccelerometerCalibrator calibrator =
31082 new KnownPositionAccelerometerCalibrator();
31083
31084 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31085
31086
31087 final Matrix ma = generateMaGeneral();
31088 final double mxy = ma.getElementAt(0, 1);
31089
31090 calibrator.setInitialMxy(mxy);
31091
31092
31093 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31094 }
31095
31096 @Test
31097 public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
31098 final KnownPositionAccelerometerCalibrator calibrator =
31099 new KnownPositionAccelerometerCalibrator();
31100
31101 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31102
31103
31104 final Matrix ma = generateMaGeneral();
31105 final double mxz = ma.getElementAt(0, 2);
31106
31107 calibrator.setInitialMxz(mxz);
31108
31109
31110 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31111 }
31112
31113 @Test
31114 public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
31115 final KnownPositionAccelerometerCalibrator calibrator =
31116 new KnownPositionAccelerometerCalibrator();
31117
31118 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31119
31120
31121 final Matrix ma = generateMaGeneral();
31122 final double myx = ma.getElementAt(1, 0);
31123
31124 calibrator.setInitialMyx(myx);
31125
31126
31127 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31128 }
31129
31130 @Test
31131 public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
31132 final KnownPositionAccelerometerCalibrator calibrator =
31133 new KnownPositionAccelerometerCalibrator();
31134
31135 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31136
31137
31138 final Matrix ma = generateMaGeneral();
31139 final double myz = ma.getElementAt(1, 2);
31140
31141 calibrator.setInitialMyz(myz);
31142
31143
31144 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31145 }
31146
31147 @Test
31148 public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
31149 final KnownPositionAccelerometerCalibrator calibrator =
31150 new KnownPositionAccelerometerCalibrator();
31151
31152 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31153
31154
31155 final Matrix ma = generateMaGeneral();
31156 final double mzx = ma.getElementAt(2, 0);
31157
31158 calibrator.setInitialMzx(mzx);
31159
31160
31161 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31162 }
31163
31164 @Test
31165 public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
31166 final KnownPositionAccelerometerCalibrator calibrator =
31167 new KnownPositionAccelerometerCalibrator();
31168
31169 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31170
31171
31172 final Matrix ma = generateMaGeneral();
31173 final double mzy = ma.getElementAt(2, 1);
31174
31175 calibrator.setInitialMzy(mzy);
31176
31177
31178 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31179 }
31180
31181 @Test
31182 public void testSetInitialScalingFactors()
31183 throws WrongSizeException, LockedException {
31184
31185 final KnownPositionAccelerometerCalibrator calibrator =
31186 new KnownPositionAccelerometerCalibrator();
31187
31188
31189 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31190 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31191 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31192
31193
31194 final Matrix ma = generateMaGeneral();
31195 final double sx = ma.getElementAt(0, 0);
31196 final double sy = ma.getElementAt(1, 1);
31197 final double sz = ma.getElementAt(2, 2);
31198
31199 calibrator.setInitialScalingFactors(sx, sy, sz);
31200
31201
31202 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31203 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31204 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31205 }
31206
31207 @Test
31208 public void testSetInitialCrossCouplingErrors()
31209 throws WrongSizeException, LockedException {
31210
31211 final KnownPositionAccelerometerCalibrator calibrator =
31212 new KnownPositionAccelerometerCalibrator();
31213
31214
31215 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31216 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31217 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31218 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31219 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31220 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31221
31222
31223 final Matrix ma = generateMaGeneral();
31224 final double mxy = ma.getElementAt(0, 1);
31225 final double mxz = ma.getElementAt(0, 2);
31226 final double myx = ma.getElementAt(1, 0);
31227 final double myz = ma.getElementAt(1, 2);
31228 final double mzx = ma.getElementAt(2, 0);
31229 final double mzy = ma.getElementAt(2, 1);
31230
31231 calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
31232
31233
31234 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31235 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31236 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31237 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31238 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31239 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31240 }
31241
31242 @Test
31243 public void testSetInitialScalingFactorsAndCrossCouplingErrors()
31244 throws WrongSizeException, LockedException {
31245
31246 final KnownPositionAccelerometerCalibrator calibrator =
31247 new KnownPositionAccelerometerCalibrator();
31248
31249
31250 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31251 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31252 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31253 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31254 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31255 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31256 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31257 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31258 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31259
31260
31261 final Matrix ma = generateMaGeneral();
31262 final double sx = ma.getElementAt(0, 0);
31263 final double sy = ma.getElementAt(1, 1);
31264 final double sz = ma.getElementAt(2, 2);
31265 final double mxy = ma.getElementAt(0, 1);
31266 final double mxz = ma.getElementAt(0, 2);
31267 final double myx = ma.getElementAt(1, 0);
31268 final double myz = ma.getElementAt(1, 2);
31269 final double mzx = ma.getElementAt(2, 0);
31270 final double mzy = ma.getElementAt(2, 1);
31271
31272 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
31273 mxy, mxz, myx, myz, mzx, mzy);
31274
31275
31276 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31277 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31278 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31279 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31280 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31281 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31282 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31283 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31284 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31285 }
31286
31287 @Test
31288 public void testGetSetInitialBias() throws LockedException {
31289 final KnownPositionAccelerometerCalibrator calibrator =
31290 new KnownPositionAccelerometerCalibrator();
31291
31292
31293 final double[] bias1 = calibrator.getInitialBias();
31294 assertArrayEquals(bias1, new double[3], 0.0);
31295
31296
31297 final Matrix ba = generateBa();
31298 final double[] bias2 = ba.getBuffer();
31299 calibrator.setInitialBias(bias2);
31300
31301
31302 final double[] bias3 = calibrator.getInitialBias();
31303 final double[] bias4 = new double[3];
31304 calibrator.getInitialBias(bias4);
31305
31306 assertArrayEquals(bias2, bias3, 0.0);
31307 assertArrayEquals(bias2, bias4, 0.0);
31308
31309
31310 try {
31311 calibrator.getInitialBias(new double[1]);
31312 fail("IllegalArgumentException expected but not thrown");
31313 } catch (final IllegalArgumentException ignore) {
31314 }
31315 try {
31316 calibrator.setInitialBias(new double[1]);
31317 fail("IllegalArgumentException expected but not thrown");
31318 } catch (final IllegalArgumentException ignore) {
31319 }
31320 }
31321
31322 @Test
31323 public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
31324 LockedException {
31325
31326 final KnownPositionAccelerometerCalibrator calibrator =
31327 new KnownPositionAccelerometerCalibrator();
31328
31329
31330 final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
31331 assertEquals(bias1, new Matrix(3, 1));
31332
31333
31334 final Matrix bias2 = generateBa();
31335 calibrator.setInitialBias(bias2);
31336
31337
31338 final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
31339 final Matrix bias4 = new Matrix(3, 1);
31340 calibrator.getInitialBiasAsMatrix(bias4);
31341
31342 assertEquals(bias2, bias3);
31343 assertEquals(bias2, bias4);
31344
31345
31346 try {
31347 calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
31348 fail("IllegalArgumentException expected but not thrown");
31349 } catch (final IllegalArgumentException ignore) {
31350 }
31351 try {
31352 calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
31353 fail("IllegalArgumentException expected but not thrown");
31354 } catch (final IllegalArgumentException ignore) {
31355 }
31356 try {
31357 calibrator.setInitialBias(new Matrix(1, 1));
31358 fail("IllegalArgumentException expected but not thrown");
31359 } catch (final IllegalArgumentException ignore) {
31360 }
31361 try {
31362 calibrator.setInitialBias(new Matrix(3, 3));
31363 fail("IllegalArgumentException expected but not thrown");
31364 } catch (final IllegalArgumentException ignore) {
31365 }
31366 }
31367
31368 @Test
31369 public void testGetSetInitialMa() throws WrongSizeException, LockedException {
31370 final KnownPositionAccelerometerCalibrator calibrator =
31371 new KnownPositionAccelerometerCalibrator();
31372
31373
31374 final Matrix ma1 = calibrator.getInitialMa();
31375 assertEquals(ma1, new Matrix(3, 3));
31376
31377
31378 final Matrix ma2 = generateMaGeneral();
31379 calibrator.setInitialMa(ma2);
31380
31381
31382 final Matrix ma3 = calibrator.getInitialMa();
31383 final Matrix ma4 = new Matrix(3, 3);
31384 calibrator.getInitialMa(ma4);
31385
31386 assertEquals(ma2, ma3);
31387 assertEquals(ma2, ma4);
31388
31389
31390 try {
31391 calibrator.getInitialMa(new Matrix(1, 3));
31392 fail("IllegalArgumentException expected but not thrown");
31393 } catch (final IllegalArgumentException ignore) {
31394 }
31395 try {
31396 calibrator.getInitialMa(new Matrix(3, 1));
31397 fail("IllegalArgumentException expected but not thrown");
31398 } catch (final IllegalArgumentException ignore) {
31399 }
31400 try {
31401 calibrator.setInitialMa(new Matrix(1, 3));
31402 fail("IllegalArgumentException expected but not thrown");
31403 } catch (final IllegalArgumentException ignore) {
31404 }
31405 try {
31406 calibrator.setInitialMa(new Matrix(3, 1));
31407 fail("IllegalArgumentException expected but not thrown");
31408 } catch (final IllegalArgumentException ignore) {
31409 }
31410 }
31411
31412 @Test
31413 public void testGetSetEcefPosition() throws LockedException {
31414 final KnownPositionAccelerometerCalibrator calibrator =
31415 new KnownPositionAccelerometerCalibrator();
31416
31417
31418 assertNull(calibrator.getEcefPosition());
31419
31420
31421 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31422 final double latitude = Math.toRadians(
31423 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31424 final double longitude = Math.toRadians(
31425 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31426 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31427 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31428 final NEDVelocity nedVelocity = new NEDVelocity();
31429 final ECEFPosition ecefPosition = new ECEFPosition();
31430 final ECEFVelocity ecefVelocity = new ECEFVelocity();
31431 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31432 ecefPosition, ecefVelocity);
31433
31434 calibrator.setPosition(ecefPosition);
31435
31436
31437 assertSame(calibrator.getEcefPosition(), ecefPosition);
31438 }
31439
31440 @Test
31441 public void testGetSetNedPosition() throws LockedException {
31442 final KnownPositionAccelerometerCalibrator calibrator =
31443 new KnownPositionAccelerometerCalibrator();
31444
31445
31446 assertNull(calibrator.getNedPosition());
31447 assertFalse(calibrator.getNedPosition(null));
31448
31449
31450 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31451 final double latitude = Math.toRadians(
31452 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31453 final double longitude = Math.toRadians(
31454 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31455 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31456 final NEDPosition nedPosition1 = new NEDPosition(latitude, longitude, height);
31457
31458 calibrator.setPosition(nedPosition1);
31459
31460
31461 final NEDPosition nedPosition2 = calibrator.getNedPosition();
31462 final NEDPosition nedPosition3 = new NEDPosition();
31463 calibrator.getNedPosition(nedPosition3);
31464
31465 assertTrue(nedPosition1.equals(nedPosition2, ABSOLUTE_ERROR));
31466 assertTrue(nedPosition1.equals(nedPosition3, ABSOLUTE_ERROR));
31467 }
31468
31469 @Test
31470 public void testGetSetMeasurements() throws LockedException {
31471 final KnownPositionAccelerometerCalibrator calibrator =
31472 new KnownPositionAccelerometerCalibrator();
31473
31474
31475 assertNull(calibrator.getMeasurements());
31476
31477
31478 final Collection<StandardDeviationBodyKinematics> measurements =
31479 Collections.emptyList();
31480 calibrator.setMeasurements(measurements);
31481
31482
31483 assertSame(calibrator.getMeasurements(), measurements);
31484 }
31485
31486 @Test
31487 public void testIsSetCommonAxisUsed() throws LockedException {
31488 final KnownPositionAccelerometerCalibrator calibrator =
31489 new KnownPositionAccelerometerCalibrator();
31490
31491
31492 assertFalse(calibrator.isCommonAxisUsed());
31493
31494
31495 calibrator.setCommonAxisUsed(true);
31496
31497
31498 assertTrue(calibrator.isCommonAxisUsed());
31499 }
31500
31501 @Test
31502 public void testGetSetListener() throws LockedException {
31503 final KnownPositionAccelerometerCalibrator calibrator =
31504 new KnownPositionAccelerometerCalibrator();
31505
31506
31507 assertNull(calibrator.getListener());
31508
31509
31510 calibrator.setListener(this);
31511
31512
31513 assertSame(calibrator.getListener(), this);
31514 }
31515
31516 @Test
31517 public void testIsReady() throws LockedException {
31518 final KnownPositionAccelerometerCalibrator calibrator =
31519 new KnownPositionAccelerometerCalibrator();
31520
31521
31522 assertFalse(calibrator.isReady());
31523
31524
31525
31526 final Collection<StandardDeviationBodyKinematics> measurements1 =
31527 Collections.emptyList();
31528 calibrator.setMeasurements(measurements1);
31529
31530
31531 assertFalse(calibrator.isReady());
31532
31533
31534
31535 calibrator.setCommonAxisUsed(false);
31536
31537 final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
31538 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31539 measurements2.add(new StandardDeviationBodyKinematics());
31540 }
31541 calibrator.setMeasurements(measurements2);
31542
31543
31544 assertFalse(calibrator.isReady());
31545
31546
31547
31548 final ECEFPosition position = new ECEFPosition();
31549 calibrator.setPosition(position);
31550
31551 assertTrue(calibrator.isReady());
31552
31553
31554
31555 measurements2.clear();
31556 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31557 measurements2.add(new StandardDeviationBodyKinematics());
31558 }
31559 calibrator.setMeasurements(measurements2);
31560
31561
31562 assertFalse(calibrator.isReady());
31563
31564 calibrator.setCommonAxisUsed(true);
31565
31566 assertTrue(calibrator.isReady());
31567 }
31568
31569 @Test
31570 public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
31571 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31572 LockedException, NotReadyException {
31573
31574 int numValid = 0;
31575 for (int t = 0; t < TIMES; t++) {
31576 final Matrix ba = generateBa();
31577 final Matrix bg = generateBg();
31578 final Matrix ma = generateMaGeneral();
31579 final Matrix mg = generateMg();
31580 final Matrix gg = generateGg();
31581
31582
31583 final double accelNoiseRootPSD = 0.0;
31584 final double gyroNoiseRootPSD = 0.0;
31585 final double accelQuantLevel = 0.0;
31586 final double gyroQuantLevel = 0.0;
31587
31588 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31589 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31590
31591 final Random random = new Random();
31592 final UniformRandomizer randomizer = new UniformRandomizer(random);
31593 final double latitude = Math.toRadians(
31594 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31595 final double longitude = Math.toRadians(
31596 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31597 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31598 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31599
31600 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31601 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31602 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31603
31604 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31605 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31606 final double roll = Math.toRadians(
31607 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31608 final double pitch = Math.toRadians(
31609 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31610 final double yaw = Math.toRadians(
31611 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31612 final CoordinateTransformation nedC = new CoordinateTransformation(
31613 roll, pitch, yaw, FrameType.BODY_FRAME,
31614 FrameType.LOCAL_NAVIGATION_FRAME);
31615
31616 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31617 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31618 .convertNEDtoECEFAndReturnNew(nedFrame);
31619
31620
31621
31622 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31623 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31624 ecefFrame);
31625
31626
31627
31628 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31629 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31630
31631 final StandardDeviationBodyKinematics measurement =
31632 new StandardDeviationBodyKinematics(measuredKinematics,
31633 specificForceStandardDeviation,
31634 angularRateStandardDeviation);
31635 measurements.add(measurement);
31636 }
31637
31638
31639
31640 final KnownPositionAccelerometerCalibrator calibrator =
31641 new KnownPositionAccelerometerCalibrator(nedPosition,
31642 measurements, false, ba, ma, this);
31643
31644
31645 reset();
31646 assertTrue(calibrator.isReady());
31647 assertFalse(calibrator.isRunning());
31648 assertEquals(mCalibrateStart, 0);
31649 assertEquals(mCalibrateEnd, 0);
31650
31651 try {
31652 calibrator.calibrate();
31653 } catch (final CalibrationException e) {
31654 continue;
31655 }
31656
31657
31658 assertTrue(calibrator.isReady());
31659 assertFalse(calibrator.isRunning());
31660 assertEquals(mCalibrateStart, 1);
31661 assertEquals(mCalibrateEnd, 1);
31662
31663 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31664 final Matrix estimatedMa = calibrator.getEstimatedMa();
31665
31666 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31667 continue;
31668 }
31669 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31670 continue;
31671 }
31672
31673 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31674 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31675
31676 numValid++;
31677
31678 break;
31679 }
31680
31681 assertTrue(numValid > 0);
31682 }
31683
31684 @Test
31685 public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
31686 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31687 LockedException, NotReadyException {
31688
31689 int numValid = 0;
31690 for (int t = 0; t < TIMES; t++) {
31691 final Matrix ba = generateBa();
31692 final Matrix bg = generateBg();
31693 final Matrix ma = generateMaGeneral();
31694 final Matrix mg = generateMg();
31695 final Matrix gg = generateGg();
31696 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31697 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31698 final double accelQuantLevel = 0.0;
31699 final double gyroQuantLevel = 0.0;
31700
31701 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31702 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31703
31704 final Random random = new Random();
31705 final UniformRandomizer randomizer = new UniformRandomizer(random);
31706 final double latitude = Math.toRadians(
31707 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31708 final double longitude = Math.toRadians(
31709 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31710 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31711 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31712
31713 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31714 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31715 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31716
31717 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31718 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31719 final double roll = Math.toRadians(
31720 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31721 final double pitch = Math.toRadians(
31722 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31723 final double yaw = Math.toRadians(
31724 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31725 final CoordinateTransformation nedC = new CoordinateTransformation(
31726 roll, pitch, yaw, FrameType.BODY_FRAME,
31727 FrameType.LOCAL_NAVIGATION_FRAME);
31728
31729 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31730 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31731 .convertNEDtoECEFAndReturnNew(nedFrame);
31732
31733
31734
31735 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31736 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31737 ecefFrame);
31738
31739
31740
31741 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31742 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31743
31744 final StandardDeviationBodyKinematics measurement =
31745 new StandardDeviationBodyKinematics(measuredKinematics,
31746 specificForceStandardDeviation,
31747 angularRateStandardDeviation);
31748 measurements.add(measurement);
31749 }
31750
31751
31752
31753 final KnownPositionAccelerometerCalibrator calibrator =
31754 new KnownPositionAccelerometerCalibrator(nedPosition,
31755 measurements, false, this);
31756
31757
31758 reset();
31759 assertTrue(calibrator.isReady());
31760 assertFalse(calibrator.isRunning());
31761 assertEquals(mCalibrateStart, 0);
31762 assertEquals(mCalibrateEnd, 0);
31763
31764 try {
31765 calibrator.calibrate();
31766 } catch (final CalibrationException e) {
31767 continue;
31768 }
31769
31770
31771 assertTrue(calibrator.isReady());
31772 assertFalse(calibrator.isRunning());
31773 assertEquals(mCalibrateStart, 1);
31774 assertEquals(mCalibrateEnd, 1);
31775
31776 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31777 final Matrix estimatedMa = calibrator.getEstimatedMa();
31778
31779 if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
31780 continue;
31781 }
31782 if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
31783 continue;
31784 }
31785
31786 assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
31787 assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
31788
31789 numValid++;
31790
31791 break;
31792 }
31793
31794 assertTrue(numValid > 0);
31795 }
31796
31797 @Test
31798 public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
31799 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31800 LockedException, NotReadyException {
31801
31802 int numValid = 0;
31803 for (int t = 0; t < TIMES; t++) {
31804 final Matrix ba = generateBa();
31805 final Matrix bg = generateBg();
31806 final Matrix ma = generateMaCommonAxis();
31807 final Matrix mg = generateMg();
31808 final Matrix gg = generateGg();
31809
31810
31811 final double accelNoiseRootPSD = 0.0;
31812 final double gyroNoiseRootPSD = 0.0;
31813 final double accelQuantLevel = 0.0;
31814 final double gyroQuantLevel = 0.0;
31815
31816 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31817 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31818
31819 final Random random = new Random();
31820 final UniformRandomizer randomizer = new UniformRandomizer(random);
31821 final double latitude = Math.toRadians(
31822 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31823 final double longitude = Math.toRadians(
31824 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31825 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31826 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31827
31828 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31829 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31830 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31831
31832 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31833 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31834 final double roll = Math.toRadians(
31835 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31836 final double pitch = Math.toRadians(
31837 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31838 final double yaw = Math.toRadians(
31839 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31840 final CoordinateTransformation nedC = new CoordinateTransformation(
31841 roll, pitch, yaw, FrameType.BODY_FRAME,
31842 FrameType.LOCAL_NAVIGATION_FRAME);
31843
31844 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31845 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31846 .convertNEDtoECEFAndReturnNew(nedFrame);
31847
31848
31849
31850 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31851 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31852 ecefFrame);
31853
31854
31855
31856 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31857 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31858
31859 final StandardDeviationBodyKinematics measurement =
31860 new StandardDeviationBodyKinematics(measuredKinematics,
31861 specificForceStandardDeviation,
31862 angularRateStandardDeviation);
31863 measurements.add(measurement);
31864 }
31865
31866
31867
31868 final KnownPositionAccelerometerCalibrator calibrator =
31869 new KnownPositionAccelerometerCalibrator(nedPosition,
31870 measurements, true, ba, ma, this);
31871
31872
31873 reset();
31874 assertTrue(calibrator.isReady());
31875 assertFalse(calibrator.isRunning());
31876 assertEquals(mCalibrateStart, 0);
31877 assertEquals(mCalibrateEnd, 0);
31878
31879 try {
31880 calibrator.calibrate();
31881 } catch (final CalibrationException e) {
31882 continue;
31883 }
31884
31885
31886 assertTrue(calibrator.isReady());
31887 assertFalse(calibrator.isRunning());
31888 assertEquals(mCalibrateStart, 1);
31889 assertEquals(mCalibrateEnd, 1);
31890
31891 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31892 final Matrix estimatedMa = calibrator.getEstimatedMa();
31893
31894 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31895 continue;
31896 }
31897 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31898 continue;
31899 }
31900
31901 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31902 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31903
31904 numValid++;
31905
31906 break;
31907 }
31908
31909 assertTrue(numValid > 0);
31910 }
31911
31912 @Test
31913 public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
31914 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31915 LockedException, NotReadyException {
31916
31917 int numValid = 0;
31918 for (int t = 0; t < TIMES; t++) {
31919 final Matrix ba = generateBa();
31920 final Matrix bg = generateBg();
31921 final Matrix ma = generateMaCommonAxis();
31922 final Matrix mg = generateMg();
31923 final Matrix gg = generateGg();
31924 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31925 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31926 final double accelQuantLevel = 0.0;
31927 final double gyroQuantLevel = 0.0;
31928
31929 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31930 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31931
31932 final Random random = new Random();
31933 final UniformRandomizer randomizer = new UniformRandomizer(random);
31934 final double latitude = Math.toRadians(
31935 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31936 final double longitude = Math.toRadians(
31937 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31938 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31939 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31940
31941 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31942 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31943 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31944
31945 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31946 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31947 final double roll = Math.toRadians(
31948 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31949 final double pitch = Math.toRadians(
31950 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31951 final double yaw = Math.toRadians(
31952 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31953 final CoordinateTransformation nedC = new CoordinateTransformation(
31954 roll, pitch, yaw, FrameType.BODY_FRAME,
31955 FrameType.LOCAL_NAVIGATION_FRAME);
31956
31957 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31958 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31959 .convertNEDtoECEFAndReturnNew(nedFrame);
31960
31961
31962
31963 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31964 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31965 ecefFrame);
31966
31967
31968
31969 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31970 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31971
31972 final StandardDeviationBodyKinematics measurement =
31973 new StandardDeviationBodyKinematics(measuredKinematics,
31974 specificForceStandardDeviation,
31975 angularRateStandardDeviation);
31976 measurements.add(measurement);
31977 }
31978
31979
31980
31981 final KnownPositionAccelerometerCalibrator calibrator =
31982 new KnownPositionAccelerometerCalibrator(nedPosition,
31983 measurements, true, this);
31984
31985
31986 reset();
31987 assertTrue(calibrator.isReady());
31988 assertFalse(calibrator.isRunning());
31989 assertEquals(mCalibrateStart, 0);
31990 assertEquals(mCalibrateEnd, 0);
31991
31992 try {
31993 calibrator.calibrate();
31994 } catch (final CalibrationException e) {
31995 continue;
31996 }
31997
31998
31999 assertTrue(calibrator.isReady());
32000 assertFalse(calibrator.isRunning());
32001 assertEquals(mCalibrateStart, 1);
32002 assertEquals(mCalibrateEnd, 1);
32003
32004 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32005 final Matrix estimatedMa = calibrator.getEstimatedMa();
32006
32007 if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
32008 continue;
32009 }
32010 if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
32011 continue;
32012 }
32013
32014 assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
32015 assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
32016
32017 numValid++;
32018
32019 break;
32020 }
32021
32022 assertTrue(numValid > 0);
32023 }
32024
32025 @Test
32026 public void testNorms() throws InvalidSourceAndDestinationFrameTypeException {
32027
32028
32029
32030
32031
32032
32033
32034
32035 final Random random = new Random();
32036 final UniformRandomizer randomizer = new UniformRandomizer(random);
32037 final double latitude = Math.toRadians(
32038 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
32039 final double longitude = Math.toRadians(
32040 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32041 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32042 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32043
32044 final double roll = Math.toRadians(
32045 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32046 final double pitch = Math.toRadians(
32047 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32048 final double yaw = Math.toRadians(
32049 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32050 final CoordinateTransformation nedC = new CoordinateTransformation(
32051 roll, pitch, yaw, FrameType.BODY_FRAME,
32052 FrameType.LOCAL_NAVIGATION_FRAME);
32053
32054 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32055 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32056 .convertNEDtoECEFAndReturnNew(nedFrame);
32057
32058 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32059 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32060 ecefFrame);
32061
32062 final double fNorm = trueKinematics.getSpecificForceNorm();
32063 final double wNorm = trueKinematics.getAngularRateNorm();
32064
32065 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(ecefFrame);
32066 final double g = gravity.getNorm();
32067
32068 assertEquals(g, fNorm, ABSOLUTE_ERROR);
32069 assertEquals(Constants.EARTH_ROTATION_RATE, wNorm, ABSOLUTE_ERROR);
32070 }
32071
32072 @Override
32073 public void onCalibrateStart(
32074 final KnownPositionAccelerometerCalibrator calibrator) {
32075 checkLocked(calibrator);
32076 mCalibrateStart++;
32077 }
32078
32079 @Override
32080 public void onCalibrateEnd(
32081 final KnownPositionAccelerometerCalibrator calibrator) {
32082 checkLocked(calibrator);
32083 mCalibrateEnd++;
32084 }
32085
32086 private void reset() {
32087 mCalibrateStart = 0;
32088 mCalibrateEnd = 0;
32089 }
32090
32091 private void checkLocked(final KnownPositionAccelerometerCalibrator calibrator) {
32092 assertTrue(calibrator.isRunning());
32093 try {
32094 calibrator.setInitialBiasX(0.0);
32095 fail("LockedException expected but not thrown");
32096 } catch (final LockedException ignore) {
32097 }
32098 try {
32099 calibrator.setInitialBiasY(0.0);
32100 fail("LockedException expected but not thrown");
32101 } catch (final LockedException ignore) {
32102 }
32103 try {
32104 calibrator.setInitialBiasZ(0.0);
32105 fail("LockedException expected but not thrown");
32106 } catch (final LockedException ignore) {
32107 }
32108 try {
32109 calibrator.setInitialBiasX(null);
32110 fail("LockedException expected but not thrown");
32111 } catch (final LockedException ignore) {
32112 }
32113 try {
32114 calibrator.setInitialBiasY(null);
32115 fail("LockedException expected but not thrown");
32116 } catch (final LockedException ignore) {
32117 }
32118 try {
32119 calibrator.setInitialBiasZ(null);
32120 fail("LockedException expected but not thrown");
32121 } catch (final LockedException ignore) {
32122 }
32123 try {
32124 calibrator.setInitialBias(0.0, 0.0, 0.0);
32125 fail("LockedException expected but not thrown");
32126 } catch (final LockedException ignore) {
32127 }
32128 try {
32129 calibrator.setInitialBias(null, null, null);
32130 fail("LockedException expected but not thrown");
32131 } catch (final LockedException ignore) {
32132 }
32133 try {
32134 calibrator.setInitialSx(0.0);
32135 fail("LockedException expected but not thrown");
32136 } catch (final LockedException ignore) {
32137 }
32138 try {
32139 calibrator.setInitialSy(0.0);
32140 fail("LockedException expected but not thrown");
32141 } catch (final LockedException ignore) {
32142 }
32143 try {
32144 calibrator.setInitialSz(0.0);
32145 fail("LockedException expected but not thrown");
32146 } catch (final LockedException ignore) {
32147 }
32148 try {
32149 calibrator.setInitialMxy(0.0);
32150 fail("LockedException expected but not thrown");
32151 } catch (final LockedException ignore) {
32152 }
32153 try {
32154 calibrator.setInitialMxz(0.0);
32155 fail("LockedException expected but not thrown");
32156 } catch (final LockedException ignore) {
32157 }
32158 try {
32159 calibrator.setInitialMyx(0.0);
32160 fail("LockedException expected but not thrown");
32161 } catch (final LockedException ignore) {
32162 }
32163 try {
32164 calibrator.setInitialMyz(0.0);
32165 fail("LockedException expected but not thrown");
32166 } catch (final LockedException ignore) {
32167 }
32168 try {
32169 calibrator.setInitialMzx(0.0);
32170 fail("LockedException expected but not thrown");
32171 } catch (final LockedException ignore) {
32172 }
32173 try {
32174 calibrator.setInitialMzy(0.0);
32175 fail("LockedException expected but not thrown");
32176 } catch (final LockedException ignore) {
32177 }
32178 try {
32179 calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
32180 fail("LockedException expected but not thrown");
32181 } catch (final LockedException ignore) {
32182 }
32183 try {
32184 calibrator.setInitialCrossCouplingErrors(
32185 0.0, 0.0, 0.0,
32186 0.0, 0.0, 0.0);
32187 fail("LockedException expected but not thrown");
32188 } catch (final LockedException ignore) {
32189 }
32190 try {
32191 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
32192 0.0, 0.0, 0.0,
32193 0.0, 0.0, 0.0,
32194 0.0, 0.0, 0.0);
32195 fail("LockedException expected but not thrown");
32196 } catch (final LockedException ignore) {
32197 }
32198 try {
32199 calibrator.setInitialBias((double[]) null);
32200 fail("LockedException expected but not thrown");
32201 } catch (final LockedException ignore) {
32202 }
32203 try {
32204 calibrator.setInitialBias((Matrix) null);
32205 fail("LockedException expected but not thrown");
32206 } catch (final LockedException ignore) {
32207 }
32208 try {
32209 calibrator.setInitialMa(null);
32210 fail("LockedException expected but not thrown");
32211 } catch (final LockedException ignore) {
32212 }
32213 try {
32214 calibrator.setPosition((ECEFPosition) null);
32215 fail("LockedException expected but not thrown");
32216 } catch (final LockedException ignore) {
32217 }
32218 try {
32219 calibrator.setPosition((NEDPosition) null);
32220 fail("LockedException expected but not thrown");
32221 } catch (final LockedException ignore) {
32222 }
32223 try {
32224 calibrator.setMeasurements(null);
32225 fail("LockedException expected but not thrown");
32226 } catch (final LockedException ignore) {
32227 }
32228 try {
32229 calibrator.setCommonAxisUsed(true);
32230 fail("LockedException expected but not thrown");
32231 } catch (final LockedException ignore) {
32232 }
32233 try {
32234 calibrator.setListener(this);
32235 fail("LockedException expected but not thrown");
32236 } catch (final LockedException ignore) {
32237 }
32238 try {
32239 calibrator.calibrate();
32240 fail("LockedException expected but not thrown");
32241 } catch (final LockedException ignore) {
32242 } catch (final Exception e) {
32243 fail("LockedException expected but not thrown");
32244 }
32245 }
32246
32247 private Matrix generateBa() {
32248 return Matrix.newFromArray(new double[]{
32249 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32250 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32251 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
32252 }
32253
32254 private Matrix generateBg() {
32255 return Matrix.newFromArray(new double[]{
32256 -9 * DEG_TO_RAD / 3600.0,
32257 13 * DEG_TO_RAD / 3600.0,
32258 -8 * DEG_TO_RAD / 3600.0});
32259 }
32260
32261 private Matrix generateMaGeneral() throws WrongSizeException {
32262 final Matrix result = new Matrix(3, 3);
32263 result.fromArray(new double[]{
32264 500e-6, -300e-6, 200e-6,
32265 -150e-6, -600e-6, 250e-6,
32266 -250e-6, 100e-6, 450e-6
32267 }, false);
32268
32269 return result;
32270 }
32271
32272 private Matrix generateMaCommonAxis() throws WrongSizeException {
32273 final Matrix result = new Matrix(3, 3);
32274 result.fromArray(new double[]{
32275 500e-6, -300e-6, 200e-6,
32276 0.0, -600e-6, 250e-6,
32277 0.0, 0.0, 450e-6
32278 }, false);
32279
32280 return result;
32281 }
32282
32283 private Matrix generateMg() throws WrongSizeException {
32284 final Matrix result = new Matrix(3, 3);
32285 result.fromArray(new double[]{
32286 400e-6, -300e-6, 250e-6,
32287 0.0, -300e-6, -150e-6,
32288 0.0, 0.0, -350e-6
32289 }, false);
32290
32291 return result;
32292 }
32293
32294 private Matrix generateGg() throws WrongSizeException {
32295 final Matrix result = new Matrix(3, 3);
32296 final double tmp = DEG_TO_RAD / (3600 * 9.80665);
32297 result.fromArray(new double[]{
32298 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
32299 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
32300 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
32301 }, false);
32302
32303 return result;
32304 }
32305
32306 private double getAccelNoiseRootPSD() {
32307 return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
32308 }
32309
32310 private double getGyroNoiseRootPSD() {
32311 return 0.01 * DEG_TO_RAD / 60.0;
32312 }
32313 }